View Javadoc
1   /*
2    * Copyright (C) 2020 Alberto Irurueta Carro (alberto@irurueta.com)
3    *
4    * Licensed under the Apache License, Version 2.0 (the "License");
5    * you may not use this file except in compliance with the License.
6    * You may obtain a copy of the License at
7    *
8    *         http://www.apache.org/licenses/LICENSE-2.0
9    *
10   * Unless required by applicable law or agreed to in writing, software
11   * distributed under the License is distributed on an "AS IS" BASIS,
12   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13   * See the License for the specific language governing permissions and
14   * limitations under the License.
15   */
16  package com.irurueta.navigation.inertial.calibration.accelerometer;
17  
18  import com.irurueta.algebra.Matrix;
19  import com.irurueta.algebra.WrongSizeException;
20  import com.irurueta.navigation.LockedException;
21  import com.irurueta.navigation.NotReadyException;
22  import com.irurueta.navigation.frames.CoordinateTransformation;
23  import com.irurueta.navigation.frames.ECEFPosition;
24  import com.irurueta.navigation.frames.ECEFVelocity;
25  import com.irurueta.navigation.frames.FrameType;
26  import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
27  import com.irurueta.navigation.frames.NEDFrame;
28  import com.irurueta.navigation.frames.NEDPosition;
29  import com.irurueta.navigation.frames.NEDVelocity;
30  import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
31  import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
32  import com.irurueta.navigation.inertial.calibration.AccelerationTriad;
33  import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
34  import com.irurueta.navigation.inertial.calibration.CalibrationException;
35  import com.irurueta.navigation.inertial.calibration.IMUErrors;
36  import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
37  import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
38  import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
39  import com.irurueta.statistics.UniformRandomizer;
40  import com.irurueta.units.Acceleration;
41  import com.irurueta.units.AccelerationUnit;
42  import org.junit.jupiter.api.Test;
43  
44  import java.util.ArrayList;
45  import java.util.Collections;
46  import java.util.Random;
47  
48  import static org.junit.jupiter.api.Assertions.*;
49  
50  class KnownBiasAndGravityNormAccelerometerCalibratorTest implements
51          KnownBiasAndGravityNormAccelerometerCalibratorListener {
52  
53      private static final double TIME_INTERVAL_SECONDS = 0.02;
54  
55      private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
56      private static final double DEG_TO_RAD = 0.01745329252;
57  
58      private static final double MIN_ANGLE_DEGREES = -180.0;
59      private static final double MAX_ANGLE_DEGREES = 180.0;
60  
61      private static final double MIN_LATITUDE_DEGREES = -90.0;
62      private static final double MAX_LATITUDE_DEGREES = 90.0;
63      private static final double MIN_LONGITUDE_DEGREES = -180.0;
64      private static final double MAX_LONGITUDE_DEGREES = 180.0;
65      private static final double MIN_HEIGHT = -50.0;
66      private static final double MAX_HEIGHT = 50.0;
67  
68      private static final int LARGE_MEASUREMENT_NUMBER = 70000;
69  
70      private static final double ABSOLUTE_ERROR = 1e-8;
71      private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
72  
73      private static final int TIMES = 70;
74  
75      private int calibrateStart;
76      private int calibrateEnd;
77  
78      @Test
79      void testConstructor1() throws WrongSizeException {
80          final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
81  
82          // check default values
83          assertEquals(0.0, calibrator.getBiasX(), 0.0);
84          assertEquals(0.0, calibrator.getBiasY(), 0.0);
85          assertEquals(0.0, calibrator.getBiasZ(), 0.0);
86          final var bx1 = calibrator.getBiasXAsAcceleration();
87          assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
88          assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
89          final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
90          calibrator.getBiasXAsAcceleration(bx2);
91          assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
92          assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
93          final var by1 = calibrator.getBiasYAsAcceleration();
94          assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
95          assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
96          final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
97          calibrator.getBiasYAsAcceleration(by2);
98          assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
99          assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
100         final var bz1 = calibrator.getBiasZAsAcceleration();
101         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
102         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
103         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
104         calibrator.getBiasZAsAcceleration(bz2);
105         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
106         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
107         final var biasTriad1 = calibrator.getBiasAsTriad();
108         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
109         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
110         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
111         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
112         final var biasTriad2 = new AccelerationTriad();
113         calibrator.getBiasAsTriad(biasTriad2);
114         assertEquals(biasTriad1, biasTriad2);
115         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
116         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
117         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
118         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
119         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
120         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
121         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
122         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
123         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
124         final var bias1 = calibrator.getBias();
125         assertArrayEquals(new double[3], bias1, 0.0);
126         final var bias2 = new double[3];
127         calibrator.getBias(bias2);
128         assertArrayEquals(bias1, bias2, 0.0);
129         final var b1 = calibrator.getBiasAsMatrix();
130         assertEquals(new Matrix(3, 1), b1);
131         final var b2 = new Matrix(3, 1);
132         calibrator.getBiasAsMatrix(b2);
133         assertEquals(b1, b2);
134         final var ma1 = calibrator.getInitialMa();
135         assertEquals(new Matrix(3, 3), ma1);
136         final var ma2 = new Matrix(3, 3);
137         calibrator.getInitialMa(ma2);
138         assertEquals(ma1, ma2);
139         assertNull(calibrator.getMeasurements());
140         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
141                 calibrator.getMeasurementType());
142         assertFalse(calibrator.isOrderedMeasurementsRequired());
143         assertFalse(calibrator.isQualityScoresRequired());
144         assertFalse(calibrator.isCommonAxisUsed());
145         assertNull(calibrator.getListener());
146         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
147         assertFalse(calibrator.isReady());
148         assertFalse(calibrator.isRunning());
149         assertNull(calibrator.getEstimatedMa());
150         assertNull(calibrator.getEstimatedSx());
151         assertNull(calibrator.getEstimatedSy());
152         assertNull(calibrator.getEstimatedSz());
153         assertNull(calibrator.getEstimatedMxy());
154         assertNull(calibrator.getEstimatedMxz());
155         assertNull(calibrator.getEstimatedMyx());
156         assertNull(calibrator.getEstimatedMyz());
157         assertNull(calibrator.getEstimatedMzx());
158         assertNull(calibrator.getEstimatedMzy());
159         assertNull(calibrator.getEstimatedCovariance());
160         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
161         assertNull(calibrator.getGroundTruthGravityNorm());
162         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
163         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
164         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
165     }
166 
167     @Test
168     void testConstructor2() throws WrongSizeException {
169         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(this);
170 
171         // check default values
172         assertEquals(0.0, calibrator.getBiasX(), 0.0);
173         assertEquals(0.0, calibrator.getBiasY(), 0.0);
174         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
175         final var bx1 = calibrator.getBiasXAsAcceleration();
176         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
177         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
178         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
179         calibrator.getBiasXAsAcceleration(bx2);
180         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
181         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
182         final var by1 = calibrator.getBiasYAsAcceleration();
183         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
184         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
185         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
186         calibrator.getBiasYAsAcceleration(by2);
187         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
188         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
189         final var bz1 = calibrator.getBiasZAsAcceleration();
190         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
191         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
192         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
193         calibrator.getBiasZAsAcceleration(bz2);
194         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
195         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
196         final var biasTriad1 = calibrator.getBiasAsTriad();
197         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
198         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
199         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
200         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
201         final var biasTriad2 = new AccelerationTriad();
202         calibrator.getBiasAsTriad(biasTriad2);
203         assertEquals(biasTriad1, biasTriad2);
204         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
205         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
206         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
207         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
208         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
209         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
210         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
211         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
212         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
213         final var bias1 = calibrator.getBias();
214         assertArrayEquals(new double[3], bias1, 0.0);
215         final var bias2 = new double[3];
216         calibrator.getBias(bias2);
217         assertArrayEquals(bias1, bias2, 0.0);
218         final var b1 = calibrator.getBiasAsMatrix();
219         assertEquals(new Matrix(3, 1), b1);
220         final var b2 = new Matrix(3, 1);
221         calibrator.getBiasAsMatrix(b2);
222         assertEquals(b1, b2);
223         final var ma1 = calibrator.getInitialMa();
224         assertEquals(new Matrix(3, 3), ma1);
225         final var ma2 = new Matrix(3, 3);
226         calibrator.getInitialMa(ma2);
227         assertEquals(ma1, ma2);
228         assertNull(calibrator.getMeasurements());
229         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
230                 calibrator.getMeasurementType());
231         assertFalse(calibrator.isOrderedMeasurementsRequired());
232         assertFalse(calibrator.isQualityScoresRequired());
233         assertFalse(calibrator.isCommonAxisUsed());
234         assertSame(this, calibrator.getListener());
235         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
236         assertFalse(calibrator.isReady());
237         assertFalse(calibrator.isRunning());
238         assertNull(calibrator.getEstimatedMa());
239         assertNull(calibrator.getEstimatedSx());
240         assertNull(calibrator.getEstimatedSy());
241         assertNull(calibrator.getEstimatedSz());
242         assertNull(calibrator.getEstimatedMxy());
243         assertNull(calibrator.getEstimatedMxz());
244         assertNull(calibrator.getEstimatedMyx());
245         assertNull(calibrator.getEstimatedMyz());
246         assertNull(calibrator.getEstimatedMzx());
247         assertNull(calibrator.getEstimatedMzy());
248         assertNull(calibrator.getEstimatedCovariance());
249         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
250         assertNull(calibrator.getGroundTruthGravityNorm());
251         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
252         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
253         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
254     }
255 
256     @Test
257     void testConstructor3() throws WrongSizeException {
258         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
259         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements);
260 
261         // check default values
262         assertEquals(0.0, calibrator.getBiasX(), 0.0);
263         assertEquals(0.0, calibrator.getBiasY(), 0.0);
264         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
265         final var bx1 = calibrator.getBiasXAsAcceleration();
266         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
267         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
268         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
269         calibrator.getBiasXAsAcceleration(bx2);
270         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
271         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
272         final var by1 = calibrator.getBiasYAsAcceleration();
273         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
274         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
275         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
276         calibrator.getBiasYAsAcceleration(by2);
277         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
278         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
279         final var bz1 = calibrator.getBiasZAsAcceleration();
280         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
281         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
282         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
283         calibrator.getBiasZAsAcceleration(bz2);
284         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
285         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
286         final var biasTriad1 = calibrator.getBiasAsTriad();
287         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
288         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
289         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
290         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
291         final var biasTriad2 = new AccelerationTriad();
292         calibrator.getBiasAsTriad(biasTriad2);
293         assertEquals(biasTriad1, biasTriad2);
294         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
295         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
296         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
297         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
298         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
299         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
300         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
301         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
302         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
303         final var bias1 = calibrator.getBias();
304         assertArrayEquals(new double[3], bias1, 0.0);
305         final var bias2 = new double[3];
306         calibrator.getBias(bias2);
307         assertArrayEquals(bias1, bias2, 0.0);
308         final var b1 = calibrator.getBiasAsMatrix();
309         assertEquals(new Matrix(3, 1), b1);
310         final var b2 = new Matrix(3, 1);
311         calibrator.getBiasAsMatrix(b2);
312         assertEquals(b1, b2);
313         final var ma1 = calibrator.getInitialMa();
314         assertEquals(new Matrix(3, 3), ma1);
315         final var ma2 = new Matrix(3, 3);
316         calibrator.getInitialMa(ma2);
317         assertEquals(ma1, ma2);
318         assertSame(measurements, calibrator.getMeasurements());
319         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
320                 calibrator.getMeasurementType());
321         assertFalse(calibrator.isOrderedMeasurementsRequired());
322         assertFalse(calibrator.isQualityScoresRequired());
323         assertFalse(calibrator.isCommonAxisUsed());
324         assertNull(calibrator.getListener());
325         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
326         assertFalse(calibrator.isReady());
327         assertFalse(calibrator.isRunning());
328         assertNull(calibrator.getEstimatedMa());
329         assertNull(calibrator.getEstimatedSx());
330         assertNull(calibrator.getEstimatedSy());
331         assertNull(calibrator.getEstimatedSz());
332         assertNull(calibrator.getEstimatedMxy());
333         assertNull(calibrator.getEstimatedMxz());
334         assertNull(calibrator.getEstimatedMyx());
335         assertNull(calibrator.getEstimatedMyz());
336         assertNull(calibrator.getEstimatedMzx());
337         assertNull(calibrator.getEstimatedMzy());
338         assertNull(calibrator.getEstimatedCovariance());
339         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
340         assertNull(calibrator.getGroundTruthGravityNorm());
341         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
342         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
343         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
344     }
345 
346     @Test
347     void testConstructor4() throws WrongSizeException {
348         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
349         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, this);
350 
351         // check default values
352         assertEquals(0.0, calibrator.getBiasX(), 0.0);
353         assertEquals(0.0, calibrator.getBiasY(), 0.0);
354         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
355         final var bx1 = calibrator.getBiasXAsAcceleration();
356         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
357         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
358         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
359         calibrator.getBiasXAsAcceleration(bx2);
360         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
361         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
362         final var by1 = calibrator.getBiasYAsAcceleration();
363         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
364         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
365         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
366         calibrator.getBiasYAsAcceleration(by2);
367         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
368         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
369         final var bz1 = calibrator.getBiasZAsAcceleration();
370         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
371         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
372         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
373         calibrator.getBiasZAsAcceleration(bz2);
374         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
375         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
376         final var biasTriad1 = calibrator.getBiasAsTriad();
377         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
378         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
379         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
380         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
381         final var biasTriad2 = new AccelerationTriad();
382         calibrator.getBiasAsTriad(biasTriad2);
383         assertEquals(biasTriad1, biasTriad2);
384         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
385         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
386         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
387         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
388         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
389         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
390         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
391         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
392         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
393         final var bias1 = calibrator.getBias();
394         assertArrayEquals(new double[3], bias1, 0.0);
395         final var bias2 = new double[3];
396         calibrator.getBias(bias2);
397         assertArrayEquals(bias1, bias2, 0.0);
398         final var b1 = calibrator.getBiasAsMatrix();
399         assertEquals(new Matrix(3, 1), b1);
400         final var b2 = new Matrix(3, 1);
401         calibrator.getBiasAsMatrix(b2);
402         assertEquals(b1, b2);
403         final var ma1 = calibrator.getInitialMa();
404         assertEquals(new Matrix(3, 3), ma1);
405         final var ma2 = new Matrix(3, 3);
406         calibrator.getInitialMa(ma2);
407         assertEquals(ma1, ma2);
408         assertSame(measurements, calibrator.getMeasurements());
409         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
410                 calibrator.getMeasurementType());
411         assertFalse(calibrator.isOrderedMeasurementsRequired());
412         assertFalse(calibrator.isQualityScoresRequired());
413         assertFalse(calibrator.isCommonAxisUsed());
414         assertSame(this, calibrator.getListener());
415         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
416         assertFalse(calibrator.isReady());
417         assertFalse(calibrator.isRunning());
418         assertNull(calibrator.getEstimatedMa());
419         assertNull(calibrator.getEstimatedSx());
420         assertNull(calibrator.getEstimatedSy());
421         assertNull(calibrator.getEstimatedSz());
422         assertNull(calibrator.getEstimatedMxy());
423         assertNull(calibrator.getEstimatedMxz());
424         assertNull(calibrator.getEstimatedMyx());
425         assertNull(calibrator.getEstimatedMyz());
426         assertNull(calibrator.getEstimatedMzx());
427         assertNull(calibrator.getEstimatedMzy());
428         assertNull(calibrator.getEstimatedCovariance());
429         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
430         assertNull(calibrator.getGroundTruthGravityNorm());
431         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
432         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
433         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
434     }
435 
436     @Test
437     void testConstructor5() throws WrongSizeException {
438         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true);
439 
440         // check default values
441         assertEquals(0.0, calibrator.getBiasX(), 0.0);
442         assertEquals(0.0, calibrator.getBiasY(), 0.0);
443         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
444         final var bx1 = calibrator.getBiasXAsAcceleration();
445         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
446         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
447         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
448         calibrator.getBiasXAsAcceleration(bx2);
449         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
450         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
451         final var by1 = calibrator.getBiasYAsAcceleration();
452         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
453         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
454         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
455         calibrator.getBiasYAsAcceleration(by2);
456         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
457         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
458         final var bz1 = calibrator.getBiasZAsAcceleration();
459         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
460         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
461         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
462         calibrator.getBiasZAsAcceleration(bz2);
463         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
464         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
465         final var biasTriad1 = calibrator.getBiasAsTriad();
466         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
467         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
468         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
469         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
470         final var biasTriad2 = new AccelerationTriad();
471         calibrator.getBiasAsTriad(biasTriad2);
472         assertEquals(biasTriad1, biasTriad2);
473         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
474         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
475         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
476         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
477         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
478         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
479         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
480         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
481         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
482         final var bias1 = calibrator.getBias();
483         assertArrayEquals(new double[3], bias1, 0.0);
484         final var bias2 = new double[3];
485         calibrator.getBias(bias2);
486         assertArrayEquals(bias1, bias2, 0.0);
487         final var b1 = calibrator.getBiasAsMatrix();
488         assertEquals(new Matrix(3, 1), b1);
489         final var b2 = new Matrix(3, 1);
490         calibrator.getBiasAsMatrix(b2);
491         assertEquals(b1, b2);
492         final var ma1 = calibrator.getInitialMa();
493         assertEquals(new Matrix(3, 3), ma1);
494         final var ma2 = new Matrix(3, 3);
495         calibrator.getInitialMa(ma2);
496         assertEquals(ma1, ma2);
497         assertNull(calibrator.getMeasurements());
498         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
499                 calibrator.getMeasurementType());
500         assertFalse(calibrator.isOrderedMeasurementsRequired());
501         assertFalse(calibrator.isQualityScoresRequired());
502         assertTrue(calibrator.isCommonAxisUsed());
503         assertNull(calibrator.getListener());
504         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
505         assertFalse(calibrator.isReady());
506         assertFalse(calibrator.isRunning());
507         assertNull(calibrator.getEstimatedMa());
508         assertNull(calibrator.getEstimatedSx());
509         assertNull(calibrator.getEstimatedSy());
510         assertNull(calibrator.getEstimatedSz());
511         assertNull(calibrator.getEstimatedMxy());
512         assertNull(calibrator.getEstimatedMxz());
513         assertNull(calibrator.getEstimatedMyx());
514         assertNull(calibrator.getEstimatedMyz());
515         assertNull(calibrator.getEstimatedMzx());
516         assertNull(calibrator.getEstimatedMzy());
517         assertNull(calibrator.getEstimatedCovariance());
518         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
519         assertNull(calibrator.getGroundTruthGravityNorm());
520         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
521         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
522         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
523     }
524 
525     @Test
526     void testConstructor6() throws WrongSizeException {
527         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, this);
528 
529         // check default values
530         assertEquals(0.0, calibrator.getBiasX(), 0.0);
531         assertEquals(0.0, calibrator.getBiasY(), 0.0);
532         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
533         final var bx1 = calibrator.getBiasXAsAcceleration();
534         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
535         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
536         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
537         calibrator.getBiasXAsAcceleration(bx2);
538         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
539         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
540         final var by1 = calibrator.getBiasYAsAcceleration();
541         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
542         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
543         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
544         calibrator.getBiasYAsAcceleration(by2);
545         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
546         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
547         final var bz1 = calibrator.getBiasZAsAcceleration();
548         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
549         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
550         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
551         calibrator.getBiasZAsAcceleration(bz2);
552         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
553         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
554         final var biasTriad1 = calibrator.getBiasAsTriad();
555         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
556         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
557         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
558         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
559         final var biasTriad2 = new AccelerationTriad();
560         calibrator.getBiasAsTriad(biasTriad2);
561         assertEquals(biasTriad1, biasTriad2);
562         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
563         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
564         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
565         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
566         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
567         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
568         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
569         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
570         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
571         final var bias1 = calibrator.getBias();
572         assertArrayEquals(new double[3], bias1, 0.0);
573         final var bias2 = new double[3];
574         calibrator.getBias(bias2);
575         assertArrayEquals(bias1, bias2, 0.0);
576         final var b1 = calibrator.getBiasAsMatrix();
577         assertEquals(new Matrix(3, 1), b1);
578         final var b2 = new Matrix(3, 1);
579         calibrator.getBiasAsMatrix(b2);
580         assertEquals(b1, b2);
581         final var ma1 = calibrator.getInitialMa();
582         assertEquals(new Matrix(3, 3), ma1);
583         final var ma2 = new Matrix(3, 3);
584         calibrator.getInitialMa(ma2);
585         assertEquals(ma1, ma2);
586         assertNull(calibrator.getMeasurements());
587         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
588                 calibrator.getMeasurementType());
589         assertFalse(calibrator.isOrderedMeasurementsRequired());
590         assertFalse(calibrator.isQualityScoresRequired());
591         assertTrue(calibrator.isCommonAxisUsed());
592         assertSame(this, calibrator.getListener());
593         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
594         assertFalse(calibrator.isReady());
595         assertFalse(calibrator.isRunning());
596         assertNull(calibrator.getEstimatedMa());
597         assertNull(calibrator.getEstimatedSx());
598         assertNull(calibrator.getEstimatedSy());
599         assertNull(calibrator.getEstimatedSz());
600         assertNull(calibrator.getEstimatedMxy());
601         assertNull(calibrator.getEstimatedMxz());
602         assertNull(calibrator.getEstimatedMyx());
603         assertNull(calibrator.getEstimatedMyz());
604         assertNull(calibrator.getEstimatedMzx());
605         assertNull(calibrator.getEstimatedMzy());
606         assertNull(calibrator.getEstimatedCovariance());
607         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
608         assertNull(calibrator.getGroundTruthGravityNorm());
609         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
610         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
611         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
612     }
613 
614     @Test
615     void testConstructor7() throws WrongSizeException {
616         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
617         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true);
618 
619         // check default values
620         assertEquals(0.0, calibrator.getBiasX(), 0.0);
621         assertEquals(0.0, calibrator.getBiasY(), 0.0);
622         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
623         final var bx1 = calibrator.getBiasXAsAcceleration();
624         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
625         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
626         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
627         calibrator.getBiasXAsAcceleration(bx2);
628         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
629         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
630         final var by1 = calibrator.getBiasYAsAcceleration();
631         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
632         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
633         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
634         calibrator.getBiasYAsAcceleration(by2);
635         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
636         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
637         final var bz1 = calibrator.getBiasZAsAcceleration();
638         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
639         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
640         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
641         calibrator.getBiasZAsAcceleration(bz2);
642         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
643         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
644         final var biasTriad1 = calibrator.getBiasAsTriad();
645         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
646         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
647         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
648         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
649         final var biasTriad2 = new AccelerationTriad();
650         calibrator.getBiasAsTriad(biasTriad2);
651         assertEquals(biasTriad1, biasTriad2);
652         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
653         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
654         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
655         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
656         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
657         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
658         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
659         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
660         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
661         final var bias1 = calibrator.getBias();
662         assertArrayEquals(new double[3], bias1, 0.0);
663         final var bias2 = new double[3];
664         calibrator.getBias(bias2);
665         assertArrayEquals(bias1, bias2, 0.0);
666         final var b1 = calibrator.getBiasAsMatrix();
667         assertEquals(new Matrix(3, 1), b1);
668         final var b2 = new Matrix(3, 1);
669         calibrator.getBiasAsMatrix(b2);
670         assertEquals(b1, b2);
671         final var ma1 = calibrator.getInitialMa();
672         assertEquals(new Matrix(3, 3), ma1);
673         final var ma2 = new Matrix(3, 3);
674         calibrator.getInitialMa(ma2);
675         assertEquals(ma1, ma2);
676         assertSame(measurements, calibrator.getMeasurements());
677         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
678                 calibrator.getMeasurementType());
679         assertFalse(calibrator.isOrderedMeasurementsRequired());
680         assertFalse(calibrator.isQualityScoresRequired());
681         assertTrue(calibrator.isCommonAxisUsed());
682         assertNull(calibrator.getListener());
683         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
684         assertFalse(calibrator.isReady());
685         assertFalse(calibrator.isRunning());
686         assertNull(calibrator.getEstimatedMa());
687         assertNull(calibrator.getEstimatedSx());
688         assertNull(calibrator.getEstimatedSy());
689         assertNull(calibrator.getEstimatedSz());
690         assertNull(calibrator.getEstimatedMxy());
691         assertNull(calibrator.getEstimatedMxz());
692         assertNull(calibrator.getEstimatedMyx());
693         assertNull(calibrator.getEstimatedMyz());
694         assertNull(calibrator.getEstimatedMzx());
695         assertNull(calibrator.getEstimatedMzy());
696         assertNull(calibrator.getEstimatedCovariance());
697         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
698         assertNull(calibrator.getGroundTruthGravityNorm());
699         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
700         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
701         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
702     }
703 
704     @Test
705     void testConstructor8() throws WrongSizeException {
706         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
707         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
708                 this);
709 
710         // check default values
711         assertEquals(0.0, calibrator.getBiasX(), 0.0);
712         assertEquals(0.0, calibrator.getBiasY(), 0.0);
713         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
714         final var bx1 = calibrator.getBiasXAsAcceleration();
715         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
716         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
717         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
718         calibrator.getBiasXAsAcceleration(bx2);
719         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
720         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
721         final var by1 = calibrator.getBiasYAsAcceleration();
722         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
723         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
724         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
725         calibrator.getBiasYAsAcceleration(by2);
726         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
727         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
728         final var bz1 = calibrator.getBiasZAsAcceleration();
729         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
730         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
731         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
732         calibrator.getBiasZAsAcceleration(bz2);
733         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
734         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
735         final var biasTriad1 = calibrator.getBiasAsTriad();
736         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
737         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
738         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
739         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
740         final var biasTriad2 = new AccelerationTriad();
741         calibrator.getBiasAsTriad(biasTriad2);
742         assertEquals(biasTriad1, biasTriad2);
743         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
744         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
745         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
746         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
747         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
748         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
749         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
750         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
751         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
752         final var bias1 = calibrator.getBias();
753         assertArrayEquals(new double[3], bias1, 0.0);
754         final var bias2 = new double[3];
755         calibrator.getBias(bias2);
756         assertArrayEquals(bias1, bias2, 0.0);
757         final var b1 = calibrator.getBiasAsMatrix();
758         assertEquals(new Matrix(3, 1), b1);
759         final var b2 = new Matrix(3, 1);
760         calibrator.getBiasAsMatrix(b2);
761         assertEquals(b1, b2);
762         final var ma1 = calibrator.getInitialMa();
763         assertEquals(new Matrix(3, 3), ma1);
764         final var ma2 = new Matrix(3, 3);
765         calibrator.getInitialMa(ma2);
766         assertEquals(ma1, ma2);
767         assertSame(measurements, calibrator.getMeasurements());
768         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
769                 calibrator.getMeasurementType());
770         assertFalse(calibrator.isOrderedMeasurementsRequired());
771         assertFalse(calibrator.isQualityScoresRequired());
772         assertTrue(calibrator.isCommonAxisUsed());
773         assertSame(this, calibrator.getListener());
774         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
775         assertFalse(calibrator.isReady());
776         assertFalse(calibrator.isRunning());
777         assertNull(calibrator.getEstimatedMa());
778         assertNull(calibrator.getEstimatedSx());
779         assertNull(calibrator.getEstimatedSy());
780         assertNull(calibrator.getEstimatedSz());
781         assertNull(calibrator.getEstimatedMxy());
782         assertNull(calibrator.getEstimatedMxz());
783         assertNull(calibrator.getEstimatedMyx());
784         assertNull(calibrator.getEstimatedMyz());
785         assertNull(calibrator.getEstimatedMzx());
786         assertNull(calibrator.getEstimatedMzy());
787         assertNull(calibrator.getEstimatedCovariance());
788         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
789         assertNull(calibrator.getGroundTruthGravityNorm());
790         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
791         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
792         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
793     }
794 
795     @Test
796     void testConstructor9() throws WrongSizeException {
797         final var ba = generateBa();
798         final var biasX = ba.getElementAtIndex(0);
799         final var biasY = ba.getElementAtIndex(1);
800         final var biasZ = ba.getElementAtIndex(2);
801 
802         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ);
803 
804         // check default values
805         assertEquals(biasX, calibrator.getBiasX(), 0.0);
806         assertEquals(biasY, calibrator.getBiasY(), 0.0);
807         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
808         final var bx1 = calibrator.getBiasXAsAcceleration();
809         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
810         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
811         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
812         calibrator.getBiasXAsAcceleration(bx2);
813         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
814         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
815         final var by1 = calibrator.getBiasYAsAcceleration();
816         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
817         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
818         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
819         calibrator.getBiasYAsAcceleration(by2);
820         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
821         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
822         final var bz1 = calibrator.getBiasZAsAcceleration();
823         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
824         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
825         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
826         calibrator.getBiasZAsAcceleration(bz2);
827         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
828         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
829         final var biasTriad1 = calibrator.getBiasAsTriad();
830         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
831         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
832         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
833         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
834         final var biasTriad2 = new AccelerationTriad();
835         calibrator.getBiasAsTriad(biasTriad2);
836         assertEquals(biasTriad1, biasTriad2);
837         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
838         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
839         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
840         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
841         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
842         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
843         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
844         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
845         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
846         final var bias1 = calibrator.getBias();
847         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
848         final var bias2 = new double[3];
849         calibrator.getBias(bias2);
850         assertArrayEquals(bias1, bias2, 0.0);
851         final var b1 = calibrator.getBiasAsMatrix();
852         assertEquals(b1, ba);
853         final var b2 = new Matrix(3, 1);
854         calibrator.getBiasAsMatrix(b2);
855         assertEquals(b1, b2);
856         final var ma1 = calibrator.getInitialMa();
857         assertEquals(new Matrix(3, 3), ma1);
858         final var ma2 = new Matrix(3, 3);
859         calibrator.getInitialMa(ma2);
860         assertEquals(ma1, ma2);
861         assertNull(calibrator.getMeasurements());
862         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
863                 calibrator.getMeasurementType());
864         assertFalse(calibrator.isOrderedMeasurementsRequired());
865         assertFalse(calibrator.isQualityScoresRequired());
866         assertFalse(calibrator.isCommonAxisUsed());
867         assertNull(calibrator.getListener());
868         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
869         assertFalse(calibrator.isReady());
870         assertFalse(calibrator.isRunning());
871         assertNull(calibrator.getEstimatedMa());
872         assertNull(calibrator.getEstimatedSx());
873         assertNull(calibrator.getEstimatedSy());
874         assertNull(calibrator.getEstimatedSz());
875         assertNull(calibrator.getEstimatedMxy());
876         assertNull(calibrator.getEstimatedMxz());
877         assertNull(calibrator.getEstimatedMyx());
878         assertNull(calibrator.getEstimatedMyz());
879         assertNull(calibrator.getEstimatedMzx());
880         assertNull(calibrator.getEstimatedMzy());
881         assertNull(calibrator.getEstimatedCovariance());
882         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
883         assertNull(calibrator.getGroundTruthGravityNorm());
884         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
885         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
886         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
887     }
888 
889     @Test
890     void testConstructor10() throws WrongSizeException {
891         final var ba = generateBa();
892         final var biasX = ba.getElementAtIndex(0);
893         final var biasY = ba.getElementAtIndex(1);
894         final var biasZ = ba.getElementAtIndex(2);
895 
896         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ, this);
897 
898         // check default values
899         assertEquals(biasX, calibrator.getBiasX(), 0.0);
900         assertEquals(biasY, calibrator.getBiasY(), 0.0);
901         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
902         final var bx1 = calibrator.getBiasXAsAcceleration();
903         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
904         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
905         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
906         calibrator.getBiasXAsAcceleration(bx2);
907         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
908         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
909         final var by1 = calibrator.getBiasYAsAcceleration();
910         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
911         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
912         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
913         calibrator.getBiasYAsAcceleration(by2);
914         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
915         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
916         final var bz1 = calibrator.getBiasZAsAcceleration();
917         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
918         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
919         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
920         calibrator.getBiasZAsAcceleration(bz2);
921         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
922         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
923         final var biasTriad1 = calibrator.getBiasAsTriad();
924         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
925         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
926         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
927         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
928         final var biasTriad2 = new AccelerationTriad();
929         calibrator.getBiasAsTriad(biasTriad2);
930         assertEquals(biasTriad1, biasTriad2);
931         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
932         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
933         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
934         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
935         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
936         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
937         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
938         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
939         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
940         final var bias1 = calibrator.getBias();
941         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
942         final var bias2 = new double[3];
943         calibrator.getBias(bias2);
944         assertArrayEquals(bias1, bias2, 0.0);
945         final var b1 = calibrator.getBiasAsMatrix();
946         assertEquals(b1, ba);
947         final var b2 = new Matrix(3, 1);
948         calibrator.getBiasAsMatrix(b2);
949         assertEquals(b1, b2);
950         final var ma1 = calibrator.getInitialMa();
951         assertEquals(new Matrix(3, 3), ma1);
952         final var ma2 = new Matrix(3, 3);
953         calibrator.getInitialMa(ma2);
954         assertEquals(ma1, ma2);
955         assertNull(calibrator.getMeasurements());
956         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
957                 calibrator.getMeasurementType());
958         assertFalse(calibrator.isOrderedMeasurementsRequired());
959         assertFalse(calibrator.isQualityScoresRequired());
960         assertFalse(calibrator.isCommonAxisUsed());
961         assertSame(this, calibrator.getListener());
962         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
963         assertFalse(calibrator.isReady());
964         assertFalse(calibrator.isRunning());
965         assertNull(calibrator.getEstimatedMa());
966         assertNull(calibrator.getEstimatedSx());
967         assertNull(calibrator.getEstimatedSy());
968         assertNull(calibrator.getEstimatedSz());
969         assertNull(calibrator.getEstimatedMxy());
970         assertNull(calibrator.getEstimatedMxz());
971         assertNull(calibrator.getEstimatedMyx());
972         assertNull(calibrator.getEstimatedMyz());
973         assertNull(calibrator.getEstimatedMzx());
974         assertNull(calibrator.getEstimatedMzy());
975         assertNull(calibrator.getEstimatedCovariance());
976         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
977         assertNull(calibrator.getGroundTruthGravityNorm());
978         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
979         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
980         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
981     }
982 
983     @Test
984     void testConstructor11() throws WrongSizeException {
985         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
986 
987         final var ba = generateBa();
988         final var biasX = ba.getElementAtIndex(0);
989         final var biasY = ba.getElementAtIndex(1);
990         final var biasZ = ba.getElementAtIndex(2);
991 
992         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ);
993 
994         // check default values
995         assertEquals(biasX, calibrator.getBiasX(), 0.0);
996         assertEquals(biasY, calibrator.getBiasY(), 0.0);
997         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
998         final var bx1 = calibrator.getBiasXAsAcceleration();
999         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1000         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1001         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1002         calibrator.getBiasXAsAcceleration(bx2);
1003         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1004         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1005         final var by1 = calibrator.getBiasYAsAcceleration();
1006         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1007         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1008         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1009         calibrator.getBiasYAsAcceleration(by2);
1010         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1011         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1012         final var bz1 = calibrator.getBiasZAsAcceleration();
1013         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1014         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1015         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1016         calibrator.getBiasZAsAcceleration(bz2);
1017         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1018         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1019         final var biasTriad1 = calibrator.getBiasAsTriad();
1020         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1021         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1022         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1023         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1024         final var biasTriad2 = new AccelerationTriad();
1025         calibrator.getBiasAsTriad(biasTriad2);
1026         assertEquals(biasTriad1, biasTriad2);
1027         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1028         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1029         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1030         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1031         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1032         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1033         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1034         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1035         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1036         final var bias1 = calibrator.getBias();
1037         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1038         final var bias2 = new double[3];
1039         calibrator.getBias(bias2);
1040         assertArrayEquals(bias1, bias2, 0.0);
1041         final var b1 = calibrator.getBiasAsMatrix();
1042         assertEquals(b1, ba);
1043         final var b2 = new Matrix(3, 1);
1044         calibrator.getBiasAsMatrix(b2);
1045         assertEquals(b1, b2);
1046         final var ma1 = calibrator.getInitialMa();
1047         assertEquals(new Matrix(3, 3), ma1);
1048         final var ma2 = new Matrix(3, 3);
1049         calibrator.getInitialMa(ma2);
1050         assertEquals(ma1, ma2);
1051         assertSame(measurements, calibrator.getMeasurements());
1052         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1053                 calibrator.getMeasurementType());
1054         assertFalse(calibrator.isOrderedMeasurementsRequired());
1055         assertFalse(calibrator.isQualityScoresRequired());
1056         assertFalse(calibrator.isCommonAxisUsed());
1057         assertNull(calibrator.getListener());
1058         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1059         assertFalse(calibrator.isReady());
1060         assertFalse(calibrator.isRunning());
1061         assertNull(calibrator.getEstimatedMa());
1062         assertNull(calibrator.getEstimatedSx());
1063         assertNull(calibrator.getEstimatedSy());
1064         assertNull(calibrator.getEstimatedSz());
1065         assertNull(calibrator.getEstimatedMxy());
1066         assertNull(calibrator.getEstimatedMxz());
1067         assertNull(calibrator.getEstimatedMyx());
1068         assertNull(calibrator.getEstimatedMyz());
1069         assertNull(calibrator.getEstimatedMzx());
1070         assertNull(calibrator.getEstimatedMzy());
1071         assertNull(calibrator.getEstimatedCovariance());
1072         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1073         assertNull(calibrator.getGroundTruthGravityNorm());
1074         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1075         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1076         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1077     }
1078 
1079     @Test
1080     void testConstructor12() throws WrongSizeException {
1081         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1082 
1083         final var ba = generateBa();
1084         final var biasX = ba.getElementAtIndex(0);
1085         final var biasY = ba.getElementAtIndex(1);
1086         final var biasZ = ba.getElementAtIndex(2);
1087 
1088         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
1089                 this);
1090 
1091         // check default values
1092         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1093         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1094         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1095         final var bx1 = calibrator.getBiasXAsAcceleration();
1096         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1097         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1098         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1099         calibrator.getBiasXAsAcceleration(bx2);
1100         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1101         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1102         final var by1 = calibrator.getBiasYAsAcceleration();
1103         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1104         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1105         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1106         calibrator.getBiasYAsAcceleration(by2);
1107         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1108         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1109         final var bz1 = calibrator.getBiasZAsAcceleration();
1110         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1111         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1112         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1113         calibrator.getBiasZAsAcceleration(bz2);
1114         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1115         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1116         final var biasTriad1 = calibrator.getBiasAsTriad();
1117         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1118         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1119         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1120         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1121         final var biasTriad2 = new AccelerationTriad();
1122         calibrator.getBiasAsTriad(biasTriad2);
1123         assertEquals(biasTriad1, biasTriad2);
1124         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1125         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1126         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1127         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1128         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1129         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1130         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1131         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1132         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1133         final var bias1 = calibrator.getBias();
1134         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1135         final var bias2 = new double[3];
1136         calibrator.getBias(bias2);
1137         assertArrayEquals(bias1, bias2, 0.0);
1138         final var b1 = calibrator.getBiasAsMatrix();
1139         assertEquals(b1, ba);
1140         final var b2 = new Matrix(3, 1);
1141         calibrator.getBiasAsMatrix(b2);
1142         assertEquals(b1, b2);
1143         final var ma1 = calibrator.getInitialMa();
1144         assertEquals(new Matrix(3, 3), ma1);
1145         final var ma2 = new Matrix(3, 3);
1146         calibrator.getInitialMa(ma2);
1147         assertEquals(ma1, ma2);
1148         assertSame(measurements, calibrator.getMeasurements());
1149         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1150                 calibrator.getMeasurementType());
1151         assertFalse(calibrator.isOrderedMeasurementsRequired());
1152         assertFalse(calibrator.isQualityScoresRequired());
1153         assertFalse(calibrator.isCommonAxisUsed());
1154         assertSame(this, calibrator.getListener());
1155         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1156         assertFalse(calibrator.isReady());
1157         assertFalse(calibrator.isRunning());
1158         assertNull(calibrator.getEstimatedMa());
1159         assertNull(calibrator.getEstimatedSx());
1160         assertNull(calibrator.getEstimatedSy());
1161         assertNull(calibrator.getEstimatedSz());
1162         assertNull(calibrator.getEstimatedMxy());
1163         assertNull(calibrator.getEstimatedMxz());
1164         assertNull(calibrator.getEstimatedMyx());
1165         assertNull(calibrator.getEstimatedMyz());
1166         assertNull(calibrator.getEstimatedMzx());
1167         assertNull(calibrator.getEstimatedMzy());
1168         assertNull(calibrator.getEstimatedCovariance());
1169         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1170         assertNull(calibrator.getGroundTruthGravityNorm());
1171         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1172         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1173         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1174     }
1175 
1176     @Test
1177     void testConstructor13() throws WrongSizeException {
1178         final var ba = generateBa();
1179         final var biasX = ba.getElementAtIndex(0);
1180         final var biasY = ba.getElementAtIndex(1);
1181         final var biasZ = ba.getElementAtIndex(2);
1182 
1183         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
1184                 biasX, biasY, biasZ);
1185 
1186         // check default values
1187         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1188         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1189         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1190         final var bx1 = calibrator.getBiasXAsAcceleration();
1191         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1192         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1193         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1194         calibrator.getBiasXAsAcceleration(bx2);
1195         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1196         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1197         final var by1 = calibrator.getBiasYAsAcceleration();
1198         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1199         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1200         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1201         calibrator.getBiasYAsAcceleration(by2);
1202         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1203         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1204         final var bz1 = calibrator.getBiasZAsAcceleration();
1205         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1206         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1207         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1208         calibrator.getBiasZAsAcceleration(bz2);
1209         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1210         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1211         final var biasTriad1 = calibrator.getBiasAsTriad();
1212         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1213         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1214         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1215         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1216         final var biasTriad2 = new AccelerationTriad();
1217         calibrator.getBiasAsTriad(biasTriad2);
1218         assertEquals(biasTriad1, biasTriad2);
1219         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1220         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1221         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1222         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1223         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1224         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1225         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1226         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1227         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1228         final var bias1 = calibrator.getBias();
1229         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1230         final var bias2 = new double[3];
1231         calibrator.getBias(bias2);
1232         assertArrayEquals(bias1, bias2, 0.0);
1233         final var b1 = calibrator.getBiasAsMatrix();
1234         assertEquals(b1, ba);
1235         final var b2 = new Matrix(3, 1);
1236         calibrator.getBiasAsMatrix(b2);
1237         assertEquals(b1, b2);
1238         final var ma1 = calibrator.getInitialMa();
1239         assertEquals(new Matrix(3, 3), ma1);
1240         final var ma2 = new Matrix(3, 3);
1241         calibrator.getInitialMa(ma2);
1242         assertEquals(ma1, ma2);
1243         assertNull(calibrator.getMeasurements());
1244         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1245                 calibrator.getMeasurementType());
1246         assertFalse(calibrator.isOrderedMeasurementsRequired());
1247         assertFalse(calibrator.isQualityScoresRequired());
1248         assertTrue(calibrator.isCommonAxisUsed());
1249         assertNull(calibrator.getListener());
1250         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1251         assertFalse(calibrator.isReady());
1252         assertFalse(calibrator.isRunning());
1253         assertNull(calibrator.getEstimatedMa());
1254         assertNull(calibrator.getEstimatedSx());
1255         assertNull(calibrator.getEstimatedSy());
1256         assertNull(calibrator.getEstimatedSz());
1257         assertNull(calibrator.getEstimatedMxy());
1258         assertNull(calibrator.getEstimatedMxz());
1259         assertNull(calibrator.getEstimatedMyx());
1260         assertNull(calibrator.getEstimatedMyz());
1261         assertNull(calibrator.getEstimatedMzx());
1262         assertNull(calibrator.getEstimatedMzy());
1263         assertNull(calibrator.getEstimatedCovariance());
1264         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1265         assertNull(calibrator.getGroundTruthGravityNorm());
1266         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1267         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1268         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1269     }
1270 
1271     @Test
1272     void testConstructor14() throws WrongSizeException {
1273         final var ba = generateBa();
1274         final var biasX = ba.getElementAtIndex(0);
1275         final var biasY = ba.getElementAtIndex(1);
1276         final var biasZ = ba.getElementAtIndex(2);
1277 
1278         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
1279                 biasX, biasY, biasZ, this);
1280 
1281         // check default values
1282         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1283         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1284         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1285         final var bx1 = calibrator.getBiasXAsAcceleration();
1286         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1287         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1288         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1289         calibrator.getBiasXAsAcceleration(bx2);
1290         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1291         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1292         final var by1 = calibrator.getBiasYAsAcceleration();
1293         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1294         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1295         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1296         calibrator.getBiasYAsAcceleration(by2);
1297         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1298         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1299         final var bz1 = calibrator.getBiasZAsAcceleration();
1300         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1301         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1302         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1303         calibrator.getBiasZAsAcceleration(bz2);
1304         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1305         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1306         final var biasTriad1 = calibrator.getBiasAsTriad();
1307         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1308         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1309         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1310         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1311         final var biasTriad2 = new AccelerationTriad();
1312         calibrator.getBiasAsTriad(biasTriad2);
1313         assertEquals(biasTriad1, biasTriad2);
1314         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1315         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1316         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1317         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1318         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1319         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1320         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1321         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1322         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1323         final var bias1 = calibrator.getBias();
1324         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1325         final var bias2 = new double[3];
1326         calibrator.getBias(bias2);
1327         assertArrayEquals(bias1, bias2, 0.0);
1328         final var b1 = calibrator.getBiasAsMatrix();
1329         assertEquals(b1, ba);
1330         final var b2 = new Matrix(3, 1);
1331         calibrator.getBiasAsMatrix(b2);
1332         assertEquals(b1, b2);
1333         final var ma1 = calibrator.getInitialMa();
1334         assertEquals(new Matrix(3, 3), ma1);
1335         final var ma2 = new Matrix(3, 3);
1336         calibrator.getInitialMa(ma2);
1337         assertEquals(ma1, ma2);
1338         assertNull(calibrator.getMeasurements());
1339         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1340                 calibrator.getMeasurementType());
1341         assertFalse(calibrator.isOrderedMeasurementsRequired());
1342         assertFalse(calibrator.isQualityScoresRequired());
1343         assertTrue(calibrator.isCommonAxisUsed());
1344         assertSame(this, calibrator.getListener());
1345         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1346         assertFalse(calibrator.isReady());
1347         assertFalse(calibrator.isRunning());
1348         assertNull(calibrator.getEstimatedMa());
1349         assertNull(calibrator.getEstimatedSx());
1350         assertNull(calibrator.getEstimatedSy());
1351         assertNull(calibrator.getEstimatedSz());
1352         assertNull(calibrator.getEstimatedMxy());
1353         assertNull(calibrator.getEstimatedMxz());
1354         assertNull(calibrator.getEstimatedMyx());
1355         assertNull(calibrator.getEstimatedMyz());
1356         assertNull(calibrator.getEstimatedMzx());
1357         assertNull(calibrator.getEstimatedMzy());
1358         assertNull(calibrator.getEstimatedCovariance());
1359         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1360         assertNull(calibrator.getGroundTruthGravityNorm());
1361         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1362         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1363         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1364     }
1365 
1366     @Test
1367     void testConstructor15() throws WrongSizeException {
1368         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1369 
1370         final var ba = generateBa();
1371         final var biasX = ba.getElementAtIndex(0);
1372         final var biasY = ba.getElementAtIndex(1);
1373         final var biasZ = ba.getElementAtIndex(2);
1374 
1375         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
1376                 biasX, biasY, biasZ);
1377 
1378         // check default values
1379         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1380         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1381         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1382         final var bx1 = calibrator.getBiasXAsAcceleration();
1383         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1384         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1385         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1386         calibrator.getBiasXAsAcceleration(bx2);
1387         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1388         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1389         final var by1 = calibrator.getBiasYAsAcceleration();
1390         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1391         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1392         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1393         calibrator.getBiasYAsAcceleration(by2);
1394         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1395         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1396         final var bz1 = calibrator.getBiasZAsAcceleration();
1397         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1398         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1399         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1400         calibrator.getBiasZAsAcceleration(bz2);
1401         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1402         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1403         final var biasTriad1 = calibrator.getBiasAsTriad();
1404         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1405         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1406         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1407         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1408         final var biasTriad2 = new AccelerationTriad();
1409         calibrator.getBiasAsTriad(biasTriad2);
1410         assertEquals(biasTriad1, biasTriad2);
1411         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1412         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1413         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1414         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1415         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1416         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1417         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1418         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1419         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1420         final var bias1 = calibrator.getBias();
1421         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1422         final var bias2 = new double[3];
1423         calibrator.getBias(bias2);
1424         assertArrayEquals(bias1, bias2, 0.0);
1425         final var b1 = calibrator.getBiasAsMatrix();
1426         assertEquals(b1, ba);
1427         final var b2 = new Matrix(3, 1);
1428         calibrator.getBiasAsMatrix(b2);
1429         assertEquals(b1, b2);
1430         final var ma1 = calibrator.getInitialMa();
1431         assertEquals(new Matrix(3, 3), ma1);
1432         final var ma2 = new Matrix(3, 3);
1433         calibrator.getInitialMa(ma2);
1434         assertEquals(ma1, ma2);
1435         assertSame(measurements, calibrator.getMeasurements());
1436         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1437                 calibrator.getMeasurementType());
1438         assertFalse(calibrator.isOrderedMeasurementsRequired());
1439         assertFalse(calibrator.isQualityScoresRequired());
1440         assertTrue(calibrator.isCommonAxisUsed());
1441         assertNull(calibrator.getListener());
1442         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1443         assertFalse(calibrator.isReady());
1444         assertFalse(calibrator.isRunning());
1445         assertNull(calibrator.getEstimatedMa());
1446         assertNull(calibrator.getEstimatedSx());
1447         assertNull(calibrator.getEstimatedSy());
1448         assertNull(calibrator.getEstimatedSz());
1449         assertNull(calibrator.getEstimatedMxy());
1450         assertNull(calibrator.getEstimatedMxz());
1451         assertNull(calibrator.getEstimatedMyx());
1452         assertNull(calibrator.getEstimatedMyz());
1453         assertNull(calibrator.getEstimatedMzx());
1454         assertNull(calibrator.getEstimatedMzy());
1455         assertNull(calibrator.getEstimatedCovariance());
1456         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1457         assertNull(calibrator.getGroundTruthGravityNorm());
1458         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1459         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1460         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1461     }
1462 
1463     @Test
1464     void testConstructor16() throws WrongSizeException {
1465         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1466 
1467         final var ba = generateBa();
1468         final var biasX = ba.getElementAtIndex(0);
1469         final var biasY = ba.getElementAtIndex(1);
1470         final var biasZ = ba.getElementAtIndex(2);
1471 
1472         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
1473                 biasX, biasY, biasZ, this);
1474 
1475         // check default values
1476         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1477         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1478         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1479         final var bx1 = calibrator.getBiasXAsAcceleration();
1480         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1481         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1482         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1483         calibrator.getBiasXAsAcceleration(bx2);
1484         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1485         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1486         final var by1 = calibrator.getBiasYAsAcceleration();
1487         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1488         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1489         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1490         calibrator.getBiasYAsAcceleration(by2);
1491         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1492         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1493         final var bz1 = calibrator.getBiasZAsAcceleration();
1494         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1495         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1496         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1497         calibrator.getBiasZAsAcceleration(bz2);
1498         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1499         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1500         final var biasTriad1 = calibrator.getBiasAsTriad();
1501         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1502         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1503         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1504         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1505         final var biasTriad2 = new AccelerationTriad();
1506         calibrator.getBiasAsTriad(biasTriad2);
1507         assertEquals(biasTriad1, biasTriad2);
1508         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1509         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1510         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1511         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1512         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1513         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1514         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1515         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1516         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1517         final var bias1 = calibrator.getBias();
1518         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1519         final var bias2 = new double[3];
1520         calibrator.getBias(bias2);
1521         assertArrayEquals(bias1, bias2, 0.0);
1522         final var b1 = calibrator.getBiasAsMatrix();
1523         assertEquals(b1, ba);
1524         final var b2 = new Matrix(3, 1);
1525         calibrator.getBiasAsMatrix(b2);
1526         assertEquals(b1, b2);
1527         final var ma1 = calibrator.getInitialMa();
1528         assertEquals(new Matrix(3, 3), ma1);
1529         final var ma2 = new Matrix(3, 3);
1530         calibrator.getInitialMa(ma2);
1531         assertEquals(ma1, ma2);
1532         assertSame(calibrator.getMeasurements(), measurements);
1533         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1534                 calibrator.getMeasurementType());
1535         assertFalse(calibrator.isOrderedMeasurementsRequired());
1536         assertFalse(calibrator.isQualityScoresRequired());
1537         assertTrue(calibrator.isCommonAxisUsed());
1538         assertSame(this, calibrator.getListener());
1539         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
1540         assertFalse(calibrator.isReady());
1541         assertFalse(calibrator.isRunning());
1542         assertNull(calibrator.getEstimatedMa());
1543         assertNull(calibrator.getEstimatedSx());
1544         assertNull(calibrator.getEstimatedSy());
1545         assertNull(calibrator.getEstimatedSz());
1546         assertNull(calibrator.getEstimatedMxy());
1547         assertNull(calibrator.getEstimatedMxz());
1548         assertNull(calibrator.getEstimatedMyx());
1549         assertNull(calibrator.getEstimatedMyz());
1550         assertNull(calibrator.getEstimatedMzx());
1551         assertNull(calibrator.getEstimatedMzy());
1552         assertNull(calibrator.getEstimatedCovariance());
1553         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1554         assertNull(calibrator.getGroundTruthGravityNorm());
1555         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1556         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1557         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1558     }
1559 
1560     @Test
1561     void testConstructor17() throws WrongSizeException {
1562         final var ba = generateBa();
1563         final var biasX = ba.getElementAtIndex(0);
1564         final var biasY = ba.getElementAtIndex(1);
1565         final var biasZ = ba.getElementAtIndex(2);
1566 
1567         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1568         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1569         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1570 
1571         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz);
1572 
1573         // check default values
1574         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1575         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1576         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1577         final var bx1 = calibrator.getBiasXAsAcceleration();
1578         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1579         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1580         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1581         calibrator.getBiasXAsAcceleration(bx2);
1582         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1583         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1584         final var by1 = calibrator.getBiasYAsAcceleration();
1585         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1586         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1587         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1588         calibrator.getBiasYAsAcceleration(by2);
1589         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1590         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1591         final var bz1 = calibrator.getBiasZAsAcceleration();
1592         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1593         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1594         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1595         calibrator.getBiasZAsAcceleration(bz2);
1596         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1597         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1598         final var biasTriad1 = calibrator.getBiasAsTriad();
1599         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1600         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1601         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1602         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1603         final var biasTriad2 = new AccelerationTriad();
1604         calibrator.getBiasAsTriad(biasTriad2);
1605         assertEquals(biasTriad1, biasTriad2);
1606         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1607         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1608         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1609         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1610         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1611         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1612         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1613         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1614         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1615         final var bias1 = calibrator.getBias();
1616         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1617         final var bias2 = new double[3];
1618         calibrator.getBias(bias2);
1619         assertArrayEquals(bias1, bias2, 0.0);
1620         final var b1 = calibrator.getBiasAsMatrix();
1621         assertEquals(b1, ba);
1622         final var b2 = new Matrix(3, 1);
1623         calibrator.getBiasAsMatrix(b2);
1624         assertEquals(b1, b2);
1625         final var ma1 = calibrator.getInitialMa();
1626         assertEquals(new Matrix(3, 3), ma1);
1627         final var ma2 = new Matrix(3, 3);
1628         calibrator.getInitialMa(ma2);
1629         assertEquals(ma1, ma2);
1630         assertNull(calibrator.getMeasurements());
1631         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1632                 calibrator.getMeasurementType());
1633         assertFalse(calibrator.isOrderedMeasurementsRequired());
1634         assertFalse(calibrator.isQualityScoresRequired());
1635         assertFalse(calibrator.isCommonAxisUsed());
1636         assertNull(calibrator.getListener());
1637         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1638         assertFalse(calibrator.isReady());
1639         assertFalse(calibrator.isRunning());
1640         assertNull(calibrator.getEstimatedMa());
1641         assertNull(calibrator.getEstimatedSx());
1642         assertNull(calibrator.getEstimatedSy());
1643         assertNull(calibrator.getEstimatedSz());
1644         assertNull(calibrator.getEstimatedMxy());
1645         assertNull(calibrator.getEstimatedMxz());
1646         assertNull(calibrator.getEstimatedMyx());
1647         assertNull(calibrator.getEstimatedMyz());
1648         assertNull(calibrator.getEstimatedMzx());
1649         assertNull(calibrator.getEstimatedMzy());
1650         assertNull(calibrator.getEstimatedCovariance());
1651         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1652         assertNull(calibrator.getGroundTruthGravityNorm());
1653         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1654         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1655         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1656     }
1657 
1658     @Test
1659     void testConstructor18() throws WrongSizeException {
1660         final var ba = generateBa();
1661         final var biasX = ba.getElementAtIndex(0);
1662         final var biasY = ba.getElementAtIndex(1);
1663         final var biasZ = ba.getElementAtIndex(2);
1664 
1665         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1666         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1667         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1668 
1669         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, this);
1670 
1671         // check default values
1672         assertEquals(calibrator.getBiasX(), biasX, 0.0);
1673         assertEquals(calibrator.getBiasY(), biasY, 0.0);
1674         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
1675         final var bx1 = calibrator.getBiasXAsAcceleration();
1676         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1677         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1678         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1679         calibrator.getBiasXAsAcceleration(bx2);
1680         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1681         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1682         final var by1 = calibrator.getBiasYAsAcceleration();
1683         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1684         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1685         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1686         calibrator.getBiasYAsAcceleration(by2);
1687         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1688         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1689         final var bz1 = calibrator.getBiasZAsAcceleration();
1690         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1691         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1692         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1693         calibrator.getBiasZAsAcceleration(bz2);
1694         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1695         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1696         final var biasTriad1 = calibrator.getBiasAsTriad();
1697         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1698         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1699         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1700         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1701         final var biasTriad2 = new AccelerationTriad();
1702         calibrator.getBiasAsTriad(biasTriad2);
1703         assertEquals(biasTriad1, biasTriad2);
1704         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1705         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1706         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1707         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1708         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1709         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1710         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1711         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1712         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1713         final var bias1 = calibrator.getBias();
1714         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1715         final var bias2 = new double[3];
1716         calibrator.getBias(bias2);
1717         assertArrayEquals(bias1, bias2, 0.0);
1718         final var b1 = calibrator.getBiasAsMatrix();
1719         assertEquals(b1, ba);
1720         final var b2 = new Matrix(3, 1);
1721         calibrator.getBiasAsMatrix(b2);
1722         assertEquals(b1, b2);
1723         final var ma1 = calibrator.getInitialMa();
1724         assertEquals(new Matrix(3, 3), ma1);
1725         final var ma2 = new Matrix(3, 3);
1726         calibrator.getInitialMa(ma2);
1727         assertEquals(ma1, ma2);
1728         assertNull(calibrator.getMeasurements());
1729         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1730                 calibrator.getMeasurementType());
1731         assertFalse(calibrator.isOrderedMeasurementsRequired());
1732         assertFalse(calibrator.isQualityScoresRequired());
1733         assertFalse(calibrator.isCommonAxisUsed());
1734         assertSame(this, calibrator.getListener());
1735         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1736         assertFalse(calibrator.isReady());
1737         assertFalse(calibrator.isRunning());
1738         assertNull(calibrator.getEstimatedMa());
1739         assertNull(calibrator.getEstimatedSx());
1740         assertNull(calibrator.getEstimatedSy());
1741         assertNull(calibrator.getEstimatedSz());
1742         assertNull(calibrator.getEstimatedMxy());
1743         assertNull(calibrator.getEstimatedMxz());
1744         assertNull(calibrator.getEstimatedMyx());
1745         assertNull(calibrator.getEstimatedMyz());
1746         assertNull(calibrator.getEstimatedMzx());
1747         assertNull(calibrator.getEstimatedMzy());
1748         assertNull(calibrator.getEstimatedCovariance());
1749         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1750         assertNull(calibrator.getGroundTruthGravityNorm());
1751         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1752         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1753         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1754     }
1755 
1756     @Test
1757     void testConstructor19() throws WrongSizeException {
1758         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1759 
1760         final var ba = generateBa();
1761         final var biasX = ba.getElementAtIndex(0);
1762         final var biasY = ba.getElementAtIndex(1);
1763         final var biasZ = ba.getElementAtIndex(2);
1764 
1765         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1766         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1767         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1768 
1769         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz);
1770 
1771         // check default values
1772         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1773         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1774         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1775         final var bx1 = calibrator.getBiasXAsAcceleration();
1776         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1777         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1778         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1779         calibrator.getBiasXAsAcceleration(bx2);
1780         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1781         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1782         final var by1 = calibrator.getBiasYAsAcceleration();
1783         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1784         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1785         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1786         calibrator.getBiasYAsAcceleration(by2);
1787         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1788         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1789         final var bz1 = calibrator.getBiasZAsAcceleration();
1790         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1791         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1792         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1793         calibrator.getBiasZAsAcceleration(bz2);
1794         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1795         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1796         final var biasTriad1 = calibrator.getBiasAsTriad();
1797         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1798         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1799         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1800         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1801         final var biasTriad2 = new AccelerationTriad();
1802         calibrator.getBiasAsTriad(biasTriad2);
1803         assertEquals(biasTriad1, biasTriad2);
1804         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1805         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1806         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1807         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1808         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1809         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1810         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1811         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1812         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1813         final var bias1 = calibrator.getBias();
1814         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1815         final var bias2 = new double[3];
1816         calibrator.getBias(bias2);
1817         assertArrayEquals(bias1, bias2, 0.0);
1818         final var b1 = calibrator.getBiasAsMatrix();
1819         assertEquals(b1, ba);
1820         final var b2 = new Matrix(3, 1);
1821         calibrator.getBiasAsMatrix(b2);
1822         assertEquals(b1, b2);
1823         final var ma1 = calibrator.getInitialMa();
1824         assertEquals(new Matrix(3, 3), ma1);
1825         final var ma2 = new Matrix(3, 3);
1826         calibrator.getInitialMa(ma2);
1827         assertEquals(ma1, ma2);
1828         assertSame(calibrator.getMeasurements(), measurements);
1829         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1830                 calibrator.getMeasurementType());
1831         assertFalse(calibrator.isOrderedMeasurementsRequired());
1832         assertFalse(calibrator.isQualityScoresRequired());
1833         assertFalse(calibrator.isCommonAxisUsed());
1834         assertNull(calibrator.getListener());
1835         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1836         assertFalse(calibrator.isReady());
1837         assertFalse(calibrator.isRunning());
1838         assertNull(calibrator.getEstimatedMa());
1839         assertNull(calibrator.getEstimatedSx());
1840         assertNull(calibrator.getEstimatedSy());
1841         assertNull(calibrator.getEstimatedSz());
1842         assertNull(calibrator.getEstimatedMxy());
1843         assertNull(calibrator.getEstimatedMxz());
1844         assertNull(calibrator.getEstimatedMyx());
1845         assertNull(calibrator.getEstimatedMyz());
1846         assertNull(calibrator.getEstimatedMzx());
1847         assertNull(calibrator.getEstimatedMzy());
1848         assertNull(calibrator.getEstimatedCovariance());
1849         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1850         assertNull(calibrator.getGroundTruthGravityNorm());
1851         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1852         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1853         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1854     }
1855 
1856     @Test
1857     void testConstructor20() throws WrongSizeException {
1858         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
1859 
1860         final var ba = generateBa();
1861         final var biasX = ba.getElementAtIndex(0);
1862         final var biasY = ba.getElementAtIndex(1);
1863         final var biasZ = ba.getElementAtIndex(2);
1864 
1865         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1866         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1867         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1868 
1869         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz,
1870                 this);
1871 
1872         // check default values
1873         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1874         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1875         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1876         final var bx1 = calibrator.getBiasXAsAcceleration();
1877         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1878         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1879         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1880         calibrator.getBiasXAsAcceleration(bx2);
1881         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1882         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1883         final var by1 = calibrator.getBiasYAsAcceleration();
1884         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1885         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1886         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1887         calibrator.getBiasYAsAcceleration(by2);
1888         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1889         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1890         final var bz1 = calibrator.getBiasZAsAcceleration();
1891         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1892         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1893         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1894         calibrator.getBiasZAsAcceleration(bz2);
1895         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1896         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1897         final var biasTriad1 = calibrator.getBiasAsTriad();
1898         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1899         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1900         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1901         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
1902         final var biasTriad2 = new AccelerationTriad();
1903         calibrator.getBiasAsTriad(biasTriad2);
1904         assertEquals(biasTriad1, biasTriad2);
1905         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
1906         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
1907         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
1908         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
1909         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
1910         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
1911         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
1912         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
1913         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
1914         final var bias1 = calibrator.getBias();
1915         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1916         final var bias2 = new double[3];
1917         calibrator.getBias(bias2);
1918         assertArrayEquals(bias1, bias2, 0.0);
1919         final var b1 = calibrator.getBiasAsMatrix();
1920         assertEquals(b1, ba);
1921         final var b2 = new Matrix(3, 1);
1922         calibrator.getBiasAsMatrix(b2);
1923         assertEquals(b1, b2);
1924         final var ma1 = calibrator.getInitialMa();
1925         assertEquals(new Matrix(3, 3), ma1);
1926         final var ma2 = new Matrix(3, 3);
1927         calibrator.getInitialMa(ma2);
1928         assertEquals(ma1, ma2);
1929         assertSame(measurements, calibrator.getMeasurements());
1930         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
1931                 calibrator.getMeasurementType());
1932         assertFalse(calibrator.isOrderedMeasurementsRequired());
1933         assertFalse(calibrator.isQualityScoresRequired());
1934         assertFalse(calibrator.isCommonAxisUsed());
1935         assertSame(this, calibrator.getListener());
1936         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
1937         assertFalse(calibrator.isReady());
1938         assertFalse(calibrator.isRunning());
1939         assertNull(calibrator.getEstimatedMa());
1940         assertNull(calibrator.getEstimatedSx());
1941         assertNull(calibrator.getEstimatedSy());
1942         assertNull(calibrator.getEstimatedSz());
1943         assertNull(calibrator.getEstimatedMxy());
1944         assertNull(calibrator.getEstimatedMxz());
1945         assertNull(calibrator.getEstimatedMyx());
1946         assertNull(calibrator.getEstimatedMyz());
1947         assertNull(calibrator.getEstimatedMzx());
1948         assertNull(calibrator.getEstimatedMzy());
1949         assertNull(calibrator.getEstimatedCovariance());
1950         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
1951         assertNull(calibrator.getGroundTruthGravityNorm());
1952         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1953         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1954         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
1955     }
1956 
1957     @Test
1958     void testConstructor21() throws WrongSizeException {
1959         final var ba = generateBa();
1960         final var biasX = ba.getElementAtIndex(0);
1961         final var biasY = ba.getElementAtIndex(1);
1962         final var biasZ = ba.getElementAtIndex(2);
1963 
1964         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1965         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1966         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
1967 
1968         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz);
1969 
1970         // check default values
1971         assertEquals(biasX, calibrator.getBiasX(), 0.0);
1972         assertEquals(biasY, calibrator.getBiasY(), 0.0);
1973         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
1974         final var bx1 = calibrator.getBiasXAsAcceleration();
1975         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
1976         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
1977         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1978         calibrator.getBiasXAsAcceleration(bx2);
1979         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
1980         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
1981         final var by1 = calibrator.getBiasYAsAcceleration();
1982         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
1983         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
1984         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1985         calibrator.getBiasYAsAcceleration(by2);
1986         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
1987         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
1988         final var bz1 = calibrator.getBiasZAsAcceleration();
1989         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
1990         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
1991         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
1992         calibrator.getBiasZAsAcceleration(bz2);
1993         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
1994         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
1995         final var biasTriad1 = calibrator.getBiasAsTriad();
1996         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
1997         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
1998         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
1999         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2000         final var biasTriad2 = new AccelerationTriad();
2001         calibrator.getBiasAsTriad(biasTriad2);
2002         assertEquals(biasTriad1, biasTriad2);
2003         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2004         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2005         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2006         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2007         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2008         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2009         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2010         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2011         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2012         final var bias1 = calibrator.getBias();
2013         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2014         final var bias2 = new double[3];
2015         calibrator.getBias(bias2);
2016         assertArrayEquals(bias1, bias2, 0.0);
2017         final var b1 = calibrator.getBiasAsMatrix();
2018         assertEquals(b1, ba);
2019         final var b2 = new Matrix(3, 1);
2020         calibrator.getBiasAsMatrix(b2);
2021         assertEquals(b1, b2);
2022         final var ma1 = calibrator.getInitialMa();
2023         assertEquals(new Matrix(3, 3), ma1);
2024         final var ma2 = new Matrix(3, 3);
2025         calibrator.getInitialMa(ma2);
2026         assertEquals(ma1, ma2);
2027         assertNull(calibrator.getMeasurements());
2028         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2029                 calibrator.getMeasurementType());
2030         assertFalse(calibrator.isOrderedMeasurementsRequired());
2031         assertFalse(calibrator.isQualityScoresRequired());
2032         assertTrue(calibrator.isCommonAxisUsed());
2033         assertNull(calibrator.getListener());
2034         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2035         assertFalse(calibrator.isReady());
2036         assertFalse(calibrator.isRunning());
2037         assertNull(calibrator.getEstimatedMa());
2038         assertNull(calibrator.getEstimatedSx());
2039         assertNull(calibrator.getEstimatedSy());
2040         assertNull(calibrator.getEstimatedSz());
2041         assertNull(calibrator.getEstimatedMxy());
2042         assertNull(calibrator.getEstimatedMxz());
2043         assertNull(calibrator.getEstimatedMyx());
2044         assertNull(calibrator.getEstimatedMyz());
2045         assertNull(calibrator.getEstimatedMzx());
2046         assertNull(calibrator.getEstimatedMzy());
2047         assertNull(calibrator.getEstimatedCovariance());
2048         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2049         assertNull(calibrator.getGroundTruthGravityNorm());
2050         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2051         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2052         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2053     }
2054 
2055     @Test
2056     void testConstructor22() throws WrongSizeException {
2057         final var ba = generateBa();
2058         final var biasX = ba.getElementAtIndex(0);
2059         final var biasY = ba.getElementAtIndex(1);
2060         final var biasZ = ba.getElementAtIndex(2);
2061 
2062         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2063         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2064         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2065 
2066         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
2067                 this);
2068 
2069         // check default values
2070         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2071         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2072         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2073         final var bx1 = calibrator.getBiasXAsAcceleration();
2074         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2075         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2076         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2077         calibrator.getBiasXAsAcceleration(bx2);
2078         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2079         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2080         final var by1 = calibrator.getBiasYAsAcceleration();
2081         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2082         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2083         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2084         calibrator.getBiasYAsAcceleration(by2);
2085         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2086         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2087         final var bz1 = calibrator.getBiasZAsAcceleration();
2088         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2089         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2090         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2091         calibrator.getBiasZAsAcceleration(bz2);
2092         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2093         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2094         final var biasTriad1 = calibrator.getBiasAsTriad();
2095         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2096         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2097         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2098         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2099         final var biasTriad2 = new AccelerationTriad();
2100         calibrator.getBiasAsTriad(biasTriad2);
2101         assertEquals(biasTriad1, biasTriad2);
2102         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2103         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2104         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2105         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2106         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2107         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2108         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2109         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2110         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2111         final var bias1 = calibrator.getBias();
2112         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2113         final var bias2 = new double[3];
2114         calibrator.getBias(bias2);
2115         assertArrayEquals(bias1, bias2, 0.0);
2116         final var b1 = calibrator.getBiasAsMatrix();
2117         assertEquals(b1, ba);
2118         final var b2 = new Matrix(3, 1);
2119         calibrator.getBiasAsMatrix(b2);
2120         assertEquals(b1, b2);
2121         final var ma1 = calibrator.getInitialMa();
2122         assertEquals(new Matrix(3, 3), ma1);
2123         final var ma2 = new Matrix(3, 3);
2124         calibrator.getInitialMa(ma2);
2125         assertEquals(ma1, ma2);
2126         assertNull(calibrator.getMeasurements());
2127         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2128                 calibrator.getMeasurementType());
2129         assertFalse(calibrator.isOrderedMeasurementsRequired());
2130         assertFalse(calibrator.isQualityScoresRequired());
2131         assertTrue(calibrator.isCommonAxisUsed());
2132         assertSame(this, calibrator.getListener());
2133         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2134         assertFalse(calibrator.isReady());
2135         assertFalse(calibrator.isRunning());
2136         assertNull(calibrator.getEstimatedMa());
2137         assertNull(calibrator.getEstimatedSx());
2138         assertNull(calibrator.getEstimatedSy());
2139         assertNull(calibrator.getEstimatedSz());
2140         assertNull(calibrator.getEstimatedMxy());
2141         assertNull(calibrator.getEstimatedMxz());
2142         assertNull(calibrator.getEstimatedMyx());
2143         assertNull(calibrator.getEstimatedMyz());
2144         assertNull(calibrator.getEstimatedMzx());
2145         assertNull(calibrator.getEstimatedMzy());
2146         assertNull(calibrator.getEstimatedCovariance());
2147         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2148         assertNull(calibrator.getGroundTruthGravityNorm());
2149         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2150         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2151         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2152     }
2153 
2154     @Test
2155     void testConstructor23() throws WrongSizeException {
2156         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2157 
2158         final var ba = generateBa();
2159         final var biasX = ba.getElementAtIndex(0);
2160         final var biasY = ba.getElementAtIndex(1);
2161         final var biasZ = ba.getElementAtIndex(2);
2162 
2163         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2164         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2165         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2166 
2167         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2168                 bx, by, bz);
2169 
2170         // check default values
2171         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2172         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2173         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2174         final var bx1 = calibrator.getBiasXAsAcceleration();
2175         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2176         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2177         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2178         calibrator.getBiasXAsAcceleration(bx2);
2179         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2180         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2181         final var by1 = calibrator.getBiasYAsAcceleration();
2182         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2183         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2184         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2185         calibrator.getBiasYAsAcceleration(by2);
2186         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2187         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2188         final var bz1 = calibrator.getBiasZAsAcceleration();
2189         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2190         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2191         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2192         calibrator.getBiasZAsAcceleration(bz2);
2193         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2194         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2195         final var biasTriad1 = calibrator.getBiasAsTriad();
2196         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2197         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2198         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2199         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2200         final var biasTriad2 = new AccelerationTriad();
2201         calibrator.getBiasAsTriad(biasTriad2);
2202         assertEquals(biasTriad1, biasTriad2);
2203         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2204         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2205         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2206         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2207         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2208         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2209         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2210         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2211         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2212         final var bias1 = calibrator.getBias();
2213         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2214         final var bias2 = new double[3];
2215         calibrator.getBias(bias2);
2216         assertArrayEquals(bias1, bias2, 0.0);
2217         final var b1 = calibrator.getBiasAsMatrix();
2218         assertEquals(b1, ba);
2219         final var b2 = new Matrix(3, 1);
2220         calibrator.getBiasAsMatrix(b2);
2221         assertEquals(b1, b2);
2222         final var ma1 = calibrator.getInitialMa();
2223         assertEquals(new Matrix(3, 3), ma1);
2224         final var ma2 = new Matrix(3, 3);
2225         calibrator.getInitialMa(ma2);
2226         assertEquals(ma1, ma2);
2227         assertSame(measurements, calibrator.getMeasurements());
2228         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2229                 calibrator.getMeasurementType());
2230         assertFalse(calibrator.isOrderedMeasurementsRequired());
2231         assertFalse(calibrator.isQualityScoresRequired());
2232         assertTrue(calibrator.isCommonAxisUsed());
2233         assertNull(calibrator.getListener());
2234         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2235         assertFalse(calibrator.isReady());
2236         assertFalse(calibrator.isRunning());
2237         assertNull(calibrator.getEstimatedMa());
2238         assertNull(calibrator.getEstimatedSx());
2239         assertNull(calibrator.getEstimatedSy());
2240         assertNull(calibrator.getEstimatedSz());
2241         assertNull(calibrator.getEstimatedMxy());
2242         assertNull(calibrator.getEstimatedMxz());
2243         assertNull(calibrator.getEstimatedMyx());
2244         assertNull(calibrator.getEstimatedMyz());
2245         assertNull(calibrator.getEstimatedMzx());
2246         assertNull(calibrator.getEstimatedMzy());
2247         assertNull(calibrator.getEstimatedCovariance());
2248         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2249         assertNull(calibrator.getGroundTruthGravityNorm());
2250         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2251         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2252         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2253     }
2254 
2255     @Test
2256     void testConstructor24() throws WrongSizeException {
2257         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2258 
2259         final var ba = generateBa();
2260         final var biasX = ba.getElementAtIndex(0);
2261         final var biasY = ba.getElementAtIndex(1);
2262         final var biasZ = ba.getElementAtIndex(2);
2263 
2264         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2265         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2266         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
2267 
2268         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2269                 bx, by, bz, this);
2270 
2271         // check default values
2272         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2273         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2274         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2275         final var bx1 = calibrator.getBiasXAsAcceleration();
2276         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2277         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2278         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2279         calibrator.getBiasXAsAcceleration(bx2);
2280         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2281         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2282         final var by1 = calibrator.getBiasYAsAcceleration();
2283         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2284         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2285         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2286         calibrator.getBiasYAsAcceleration(by2);
2287         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2288         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2289         final var bz1 = calibrator.getBiasZAsAcceleration();
2290         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2291         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2292         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2293         calibrator.getBiasZAsAcceleration(bz2);
2294         assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2295         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2296         final var biasTriad1 = calibrator.getBiasAsTriad();
2297         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2298         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2299         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2300         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2301         final var biasTriad2 = new AccelerationTriad();
2302         calibrator.getBiasAsTriad(biasTriad2);
2303         assertEquals(biasTriad1, biasTriad2);
2304         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
2305         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
2306         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
2307         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2308         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2309         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2310         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2311         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2312         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2313         final var bias1 = calibrator.getBias();
2314         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2315         final var bias2 = new double[3];
2316         calibrator.getBias(bias2);
2317         assertArrayEquals(bias1, bias2, 0.0);
2318         final var b1 = calibrator.getBiasAsMatrix();
2319         assertEquals(b1, ba);
2320         final var b2 = new Matrix(3, 1);
2321         calibrator.getBiasAsMatrix(b2);
2322         assertEquals(b1, b2);
2323         final var ma1 = calibrator.getInitialMa();
2324         assertEquals(new Matrix(3, 3), ma1);
2325         final var ma2 = new Matrix(3, 3);
2326         calibrator.getInitialMa(ma2);
2327         assertEquals(ma1, ma2);
2328         assertSame(measurements, calibrator.getMeasurements());
2329         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2330                 calibrator.getMeasurementType());
2331         assertFalse(calibrator.isOrderedMeasurementsRequired());
2332         assertFalse(calibrator.isQualityScoresRequired());
2333         assertTrue(calibrator.isCommonAxisUsed());
2334         assertSame(this, calibrator.getListener());
2335         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2336         assertFalse(calibrator.isReady());
2337         assertFalse(calibrator.isRunning());
2338         assertNull(calibrator.getEstimatedMa());
2339         assertNull(calibrator.getEstimatedSx());
2340         assertNull(calibrator.getEstimatedSy());
2341         assertNull(calibrator.getEstimatedSz());
2342         assertNull(calibrator.getEstimatedMxy());
2343         assertNull(calibrator.getEstimatedMxz());
2344         assertNull(calibrator.getEstimatedMyx());
2345         assertNull(calibrator.getEstimatedMyz());
2346         assertNull(calibrator.getEstimatedMzx());
2347         assertNull(calibrator.getEstimatedMzy());
2348         assertNull(calibrator.getEstimatedCovariance());
2349         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2350         assertNull(calibrator.getGroundTruthGravityNorm());
2351         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2352         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2353         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2354     }
2355 
2356     @Test
2357     void testConstructor25() throws WrongSizeException {
2358         final var ba = generateBa();
2359         final var biasX = ba.getElementAtIndex(0);
2360         final var biasY = ba.getElementAtIndex(1);
2361         final var biasZ = ba.getElementAtIndex(2);
2362 
2363         final var ma = generateMaCommonAxis();
2364         final var sx = ma.getElementAt(0, 0);
2365         final var sy = ma.getElementAt(1, 1);
2366         final var sz = ma.getElementAt(2, 2);
2367 
2368         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ, sx, sy, sz);
2369 
2370         // check default values
2371         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2372         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2373         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2374         final var bx1 = calibrator.getBiasXAsAcceleration();
2375         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2376         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2377         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2378         calibrator.getBiasXAsAcceleration(bx2);
2379         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2380         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2381         final var by1 = calibrator.getBiasYAsAcceleration();
2382         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2383         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2384         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2385         calibrator.getBiasYAsAcceleration(by2);
2386         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2387         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2388         final var bz1 = calibrator.getBiasZAsAcceleration();
2389         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2390         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2391         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2392         calibrator.getBiasZAsAcceleration(bz2);
2393         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2394         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2395         final var biasTriad1 = calibrator.getBiasAsTriad();
2396         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2397         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2398         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2399         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2400         final var biasTriad2 = new AccelerationTriad();
2401         calibrator.getBiasAsTriad(biasTriad2);
2402         assertEquals(biasTriad1, biasTriad2);
2403         assertEquals(sx, calibrator.getInitialSx(), 0.0);
2404         assertEquals(sy, calibrator.getInitialSy(), 0.0);
2405         assertEquals(sz, calibrator.getInitialSz(), 0.0);
2406         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2407         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2408         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2409         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2410         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2411         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2412         final var bias1 = calibrator.getBias();
2413         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2414         final var bias2 = new double[3];
2415         calibrator.getBias(bias2);
2416         assertArrayEquals(bias1, bias2, 0.0);
2417         final var b1 = calibrator.getBiasAsMatrix();
2418         assertEquals(b1, ba);
2419         final var b2 = new Matrix(3, 1);
2420         calibrator.getBiasAsMatrix(b2);
2421         assertEquals(b1, b2);
2422         final var ma1 = calibrator.getInitialMa();
2423         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2424         final var ma2 = new Matrix(3, 3);
2425         calibrator.getInitialMa(ma2);
2426         assertEquals(ma1, ma2);
2427         assertNull(calibrator.getMeasurements());
2428         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2429                 calibrator.getMeasurementType());
2430         assertFalse(calibrator.isOrderedMeasurementsRequired());
2431         assertFalse(calibrator.isQualityScoresRequired());
2432         assertFalse(calibrator.isCommonAxisUsed());
2433         assertNull(calibrator.getListener());
2434         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
2435         assertFalse(calibrator.isReady());
2436         assertFalse(calibrator.isRunning());
2437         assertNull(calibrator.getEstimatedMa());
2438         assertNull(calibrator.getEstimatedSx());
2439         assertNull(calibrator.getEstimatedSy());
2440         assertNull(calibrator.getEstimatedSz());
2441         assertNull(calibrator.getEstimatedMxy());
2442         assertNull(calibrator.getEstimatedMxz());
2443         assertNull(calibrator.getEstimatedMyx());
2444         assertNull(calibrator.getEstimatedMyz());
2445         assertNull(calibrator.getEstimatedMzx());
2446         assertNull(calibrator.getEstimatedMzy());
2447         assertNull(calibrator.getEstimatedCovariance());
2448         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2449         assertNull(calibrator.getGroundTruthGravityNorm());
2450         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2451         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2452         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2453     }
2454 
2455     @Test
2456     void testConstructor26() throws WrongSizeException {
2457         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2458 
2459         final var ba = generateBa();
2460         final var biasX = ba.getElementAtIndex(0);
2461         final var biasY = ba.getElementAtIndex(1);
2462         final var biasZ = ba.getElementAtIndex(2);
2463 
2464         final var ma = generateMaCommonAxis();
2465         final var sx = ma.getElementAt(0, 0);
2466         final var sy = ma.getElementAt(1, 1);
2467         final var sz = ma.getElementAt(2, 2);
2468 
2469         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
2470                 sx, sy, sz);
2471 
2472         // check default values
2473         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2474         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2475         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2476         final var bx1 = calibrator.getBiasXAsAcceleration();
2477         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2478         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2479         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2480         calibrator.getBiasXAsAcceleration(bx2);
2481         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2482         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2483         final var by1 = calibrator.getBiasYAsAcceleration();
2484         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2485         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2486         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2487         calibrator.getBiasYAsAcceleration(by2);
2488         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2489         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2490         final var bz1 = calibrator.getBiasZAsAcceleration();
2491         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2492         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2493         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2494         calibrator.getBiasZAsAcceleration(bz2);
2495         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2496         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2497         final var biasTriad1 = calibrator.getBiasAsTriad();
2498         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2499         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2500         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2501         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2502         final var biasTriad2 = new AccelerationTriad();
2503         calibrator.getBiasAsTriad(biasTriad2);
2504         assertEquals(biasTriad1, biasTriad2);
2505         assertEquals(sx, calibrator.getInitialSx(), 0.0);
2506         assertEquals(sy, calibrator.getInitialSy(), 0.0);
2507         assertEquals(sz, calibrator.getInitialSz(), 0.0);
2508         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2509         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2510         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2511         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2512         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2513         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2514         final var bias1 = calibrator.getBias();
2515         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2516         final var bias2 = new double[3];
2517         calibrator.getBias(bias2);
2518         assertArrayEquals(bias1, bias2, 0.0);
2519         final var b1 = calibrator.getBiasAsMatrix();
2520         assertEquals(b1, ba);
2521         final var b2 = new Matrix(3, 1);
2522         calibrator.getBiasAsMatrix(b2);
2523         assertEquals(b1, b2);
2524         final var ma1 = calibrator.getInitialMa();
2525         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2526         final var ma2 = new Matrix(3, 3);
2527         calibrator.getInitialMa(ma2);
2528         assertEquals(ma1, ma2);
2529         assertSame(measurements, calibrator.getMeasurements());
2530         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2531                 calibrator.getMeasurementType());
2532         assertFalse(calibrator.isOrderedMeasurementsRequired());
2533         assertFalse(calibrator.isQualityScoresRequired());
2534         assertFalse(calibrator.isCommonAxisUsed());
2535         assertNull(calibrator.getListener());
2536         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
2537         assertFalse(calibrator.isReady());
2538         assertFalse(calibrator.isRunning());
2539         assertNull(calibrator.getEstimatedMa());
2540         assertNull(calibrator.getEstimatedSx());
2541         assertNull(calibrator.getEstimatedSy());
2542         assertNull(calibrator.getEstimatedSz());
2543         assertNull(calibrator.getEstimatedMxy());
2544         assertNull(calibrator.getEstimatedMxz());
2545         assertNull(calibrator.getEstimatedMyx());
2546         assertNull(calibrator.getEstimatedMyz());
2547         assertNull(calibrator.getEstimatedMzx());
2548         assertNull(calibrator.getEstimatedMzy());
2549         assertNull(calibrator.getEstimatedCovariance());
2550         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2551         assertNull(calibrator.getGroundTruthGravityNorm());
2552         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2553         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2554         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2555     }
2556 
2557     @Test
2558     void testConstructor27() throws WrongSizeException {
2559         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2560 
2561         final var ba = generateBa();
2562         final var biasX = ba.getElementAtIndex(0);
2563         final var biasY = ba.getElementAtIndex(1);
2564         final var biasZ = ba.getElementAtIndex(2);
2565 
2566         final var ma = generateMaCommonAxis();
2567         final var sx = ma.getElementAt(0, 0);
2568         final var sy = ma.getElementAt(1, 1);
2569         final var sz = ma.getElementAt(2, 2);
2570 
2571         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
2572                 sx, sy, sz, this);
2573 
2574         // check default values
2575         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2576         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2577         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2578         final var bx1 = calibrator.getBiasXAsAcceleration();
2579         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2580         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2581         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2582         calibrator.getBiasXAsAcceleration(bx2);
2583         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2584         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2585         final var by1 = calibrator.getBiasYAsAcceleration();
2586         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2587         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2588         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2589         calibrator.getBiasYAsAcceleration(by2);
2590         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2591         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2592         final var bz1 = calibrator.getBiasZAsAcceleration();
2593         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2594         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2595         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2596         calibrator.getBiasZAsAcceleration(bz2);
2597         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2598         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2599         final var biasTriad1 = calibrator.getBiasAsTriad();
2600         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2601         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2602         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2603         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2604         final var biasTriad2 = new AccelerationTriad();
2605         calibrator.getBiasAsTriad(biasTriad2);
2606         assertEquals(biasTriad1, biasTriad2);
2607         assertEquals(sx, calibrator.getInitialSx(), 0.0);
2608         assertEquals(sy, calibrator.getInitialSy(), 0.0);
2609         assertEquals(sz, calibrator.getInitialSz(), 0.0);
2610         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2611         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2612         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2613         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2614         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2615         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2616         final var bias1 = calibrator.getBias();
2617         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2618         final var bias2 = new double[3];
2619         calibrator.getBias(bias2);
2620         assertArrayEquals(bias1, bias2, 0.0);
2621         final var b1 = calibrator.getBiasAsMatrix();
2622         assertEquals(b1, ba);
2623         final var b2 = new Matrix(3, 1);
2624         calibrator.getBiasAsMatrix(b2);
2625         assertEquals(b1, b2);
2626         final var ma1 = calibrator.getInitialMa();
2627         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2628         final var ma2 = new Matrix(3, 3);
2629         calibrator.getInitialMa(ma2);
2630         assertEquals(ma1, ma2);
2631         assertSame(measurements, calibrator.getMeasurements());
2632         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2633                 calibrator.getMeasurementType());
2634         assertFalse(calibrator.isOrderedMeasurementsRequired());
2635         assertFalse(calibrator.isQualityScoresRequired());
2636         assertFalse(calibrator.isCommonAxisUsed());
2637         assertSame(this, calibrator.getListener());
2638         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
2639         assertFalse(calibrator.isReady());
2640         assertFalse(calibrator.isRunning());
2641         assertNull(calibrator.getEstimatedMa());
2642         assertNull(calibrator.getEstimatedSx());
2643         assertNull(calibrator.getEstimatedSy());
2644         assertNull(calibrator.getEstimatedSz());
2645         assertNull(calibrator.getEstimatedMxy());
2646         assertNull(calibrator.getEstimatedMxz());
2647         assertNull(calibrator.getEstimatedMyx());
2648         assertNull(calibrator.getEstimatedMyz());
2649         assertNull(calibrator.getEstimatedMzx());
2650         assertNull(calibrator.getEstimatedMzy());
2651         assertNull(calibrator.getEstimatedCovariance());
2652         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2653         assertNull(calibrator.getGroundTruthGravityNorm());
2654         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2655         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2656         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2657     }
2658 
2659     @Test
2660     void testConstructor28() throws WrongSizeException {
2661         final var ba = generateBa();
2662         final var biasX = ba.getElementAtIndex(0);
2663         final var biasY = ba.getElementAtIndex(1);
2664         final var biasZ = ba.getElementAtIndex(2);
2665 
2666         final var ma = generateMaCommonAxis();
2667         final var sx = ma.getElementAt(0, 0);
2668         final var sy = ma.getElementAt(1, 1);
2669         final var sz = ma.getElementAt(2, 2);
2670 
2671         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
2672                 biasX, biasY, biasZ, sx, sy, sz);
2673 
2674         // check default values
2675         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2676         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2677         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
2678         final var bx1 = calibrator.getBiasXAsAcceleration();
2679         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2680         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2681         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2682         calibrator.getBiasXAsAcceleration(bx2);
2683         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2684         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2685         final var by1 = calibrator.getBiasYAsAcceleration();
2686         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2687         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2688         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2689         calibrator.getBiasYAsAcceleration(by2);
2690         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2691         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2692         final var bz1 = calibrator.getBiasZAsAcceleration();
2693         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2694         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2695         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2696         calibrator.getBiasZAsAcceleration(bz2);
2697         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2698         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2699         final var biasTriad1 = calibrator.getBiasAsTriad();
2700         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2701         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2702         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2703         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2704         final var biasTriad2 = new AccelerationTriad();
2705         calibrator.getBiasAsTriad(biasTriad2);
2706         assertEquals(biasTriad1, biasTriad2);
2707         assertEquals(sx, calibrator.getInitialSx(), 0.0);
2708         assertEquals(sy, calibrator.getInitialSy(), 0.0);
2709         assertEquals(sz, calibrator.getInitialSz(), 0.0);
2710         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2711         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2712         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2713         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2714         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2715         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2716         final var bias1 = calibrator.getBias();
2717         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2718         final var bias2 = new double[3];
2719         calibrator.getBias(bias2);
2720         assertArrayEquals(bias1, bias2, 0.0);
2721         final var b1 = calibrator.getBiasAsMatrix();
2722         assertEquals(b1, ba);
2723         final var b2 = new Matrix(3, 1);
2724         calibrator.getBiasAsMatrix(b2);
2725         assertEquals(b1, b2);
2726         final var ma1 = calibrator.getInitialMa();
2727         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2728         final var ma2 = new Matrix(3, 3);
2729         calibrator.getInitialMa(ma2);
2730         assertEquals(ma1, ma2);
2731         assertNull(calibrator.getMeasurements());
2732         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2733                 calibrator.getMeasurementType());
2734         assertFalse(calibrator.isOrderedMeasurementsRequired());
2735         assertFalse(calibrator.isQualityScoresRequired());
2736         assertTrue(calibrator.isCommonAxisUsed());
2737         assertNull(calibrator.getListener());
2738         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2739         assertFalse(calibrator.isReady());
2740         assertFalse(calibrator.isRunning());
2741         assertNull(calibrator.getEstimatedMa());
2742         assertNull(calibrator.getEstimatedSx());
2743         assertNull(calibrator.getEstimatedSy());
2744         assertNull(calibrator.getEstimatedSz());
2745         assertNull(calibrator.getEstimatedMxy());
2746         assertNull(calibrator.getEstimatedMxz());
2747         assertNull(calibrator.getEstimatedMyx());
2748         assertNull(calibrator.getEstimatedMyz());
2749         assertNull(calibrator.getEstimatedMzx());
2750         assertNull(calibrator.getEstimatedMzy());
2751         assertNull(calibrator.getEstimatedCovariance());
2752         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2753         assertNull(calibrator.getGroundTruthGravityNorm());
2754         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2755         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2756         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2757     }
2758 
2759     @Test
2760     void testConstructor29() throws WrongSizeException {
2761         final var ba = generateBa();
2762         final var biasX = ba.getElementAtIndex(0);
2763         final var biasY = ba.getElementAtIndex(1);
2764         final var biasZ = ba.getElementAtIndex(2);
2765 
2766         final var ma = generateMaCommonAxis();
2767         final var sx = ma.getElementAt(0, 0);
2768         final var sy = ma.getElementAt(1, 1);
2769         final var sz = ma.getElementAt(2, 2);
2770 
2771         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
2772                 biasX, biasY, biasZ, sx, sy, sz, this);
2773 
2774         // check default values
2775         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2776         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2777         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2778         final var bx1 = calibrator.getBiasXAsAcceleration();
2779         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2780         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2781         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2782         calibrator.getBiasXAsAcceleration(bx2);
2783         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2784         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2785         final var by1 = calibrator.getBiasYAsAcceleration();
2786         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2787         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2788         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2789         calibrator.getBiasYAsAcceleration(by2);
2790         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2791         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2792         final var bz1 = calibrator.getBiasZAsAcceleration();
2793         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2794         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2795         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2796         calibrator.getBiasZAsAcceleration(bz2);
2797         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2798         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2799         final var biasTriad1 = calibrator.getBiasAsTriad();
2800         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2801         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2802         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2803         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2804         final var biasTriad2 = new AccelerationTriad();
2805         calibrator.getBiasAsTriad(biasTriad2);
2806         assertEquals(biasTriad1, biasTriad2);
2807         assertEquals(sx, calibrator.getInitialSx(), 0.0);
2808         assertEquals(sy, calibrator.getInitialSy(), 0.0);
2809         assertEquals(sz, calibrator.getInitialSz(), 0.0);
2810         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2811         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2812         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2813         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2814         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2815         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2816         final var bias1 = calibrator.getBias();
2817         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2818         final var bias2 = new double[3];
2819         calibrator.getBias(bias2);
2820         assertArrayEquals(bias1, bias2, 0.0);
2821         final var b1 = calibrator.getBiasAsMatrix();
2822         assertEquals(b1, ba);
2823         final var b2 = new Matrix(3, 1);
2824         calibrator.getBiasAsMatrix(b2);
2825         assertEquals(b1, b2);
2826         final var ma1 = calibrator.getInitialMa();
2827         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2828         final var ma2 = new Matrix(3, 3);
2829         calibrator.getInitialMa(ma2);
2830         assertEquals(ma1, ma2);
2831         assertNull(calibrator.getMeasurements());
2832         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2833                 calibrator.getMeasurementType());
2834         assertFalse(calibrator.isOrderedMeasurementsRequired());
2835         assertFalse(calibrator.isQualityScoresRequired());
2836         assertTrue(calibrator.isCommonAxisUsed());
2837         assertSame(this, calibrator.getListener());
2838         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2839         assertFalse(calibrator.isReady());
2840         assertFalse(calibrator.isRunning());
2841         assertNull(calibrator.getEstimatedMa());
2842         assertNull(calibrator.getEstimatedSx());
2843         assertNull(calibrator.getEstimatedSy());
2844         assertNull(calibrator.getEstimatedSz());
2845         assertNull(calibrator.getEstimatedMxy());
2846         assertNull(calibrator.getEstimatedMxz());
2847         assertNull(calibrator.getEstimatedMyx());
2848         assertNull(calibrator.getEstimatedMyz());
2849         assertNull(calibrator.getEstimatedMzx());
2850         assertNull(calibrator.getEstimatedMzy());
2851         assertNull(calibrator.getEstimatedCovariance());
2852         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2853         assertNull(calibrator.getGroundTruthGravityNorm());
2854         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2855         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2856         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2857     }
2858 
2859     @Test
2860     void testConstructor30() throws WrongSizeException {
2861         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2862         final var ba = generateBa();
2863         final var biasX = ba.getElementAtIndex(0);
2864         final var biasY = ba.getElementAtIndex(1);
2865         final var biasZ = ba.getElementAtIndex(2);
2866 
2867         final var ma = generateMaCommonAxis();
2868         final var sx = ma.getElementAt(0, 0);
2869         final var sy = ma.getElementAt(1, 1);
2870         final var sz = ma.getElementAt(2, 2);
2871 
2872         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2873                 biasX, biasY, biasZ, sx, sy, sz);
2874 
2875         // check default values
2876         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2877         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2878         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2879         final var bx1 = calibrator.getBiasXAsAcceleration();
2880         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2881         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2882         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2883         calibrator.getBiasXAsAcceleration(bx2);
2884         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2885         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2886         final var by1 = calibrator.getBiasYAsAcceleration();
2887         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2888         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2889         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2890         calibrator.getBiasYAsAcceleration(by2);
2891         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2892         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2893         final var bz1 = calibrator.getBiasZAsAcceleration();
2894         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2895         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2896         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2897         calibrator.getBiasZAsAcceleration(bz2);
2898         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
2899         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
2900         final var biasTriad1 = calibrator.getBiasAsTriad();
2901         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
2902         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
2903         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
2904         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
2905         final var biasTriad2 = new AccelerationTriad();
2906         calibrator.getBiasAsTriad(biasTriad2);
2907         assertEquals(biasTriad1, biasTriad2);
2908         assertEquals(sx, calibrator.getInitialSx(), 0.0);
2909         assertEquals(sy, calibrator.getInitialSy(), 0.0);
2910         assertEquals(sz, calibrator.getInitialSz(), 0.0);
2911         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
2912         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
2913         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
2914         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
2915         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
2916         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
2917         final var bias1 = calibrator.getBias();
2918         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2919         final var bias2 = new double[3];
2920         calibrator.getBias(bias2);
2921         assertArrayEquals(bias1, bias2, 0.0);
2922         final var b1 = calibrator.getBiasAsMatrix();
2923         assertEquals(b1, ba);
2924         final var b2 = new Matrix(3, 1);
2925         calibrator.getBiasAsMatrix(b2);
2926         assertEquals(b1, b2);
2927         final var ma1 = calibrator.getInitialMa();
2928         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2929         final var ma2 = new Matrix(3, 3);
2930         calibrator.getInitialMa(ma2);
2931         assertEquals(ma1, ma2);
2932         assertSame(measurements, calibrator.getMeasurements());
2933         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
2934                 calibrator.getMeasurementType());
2935         assertFalse(calibrator.isOrderedMeasurementsRequired());
2936         assertFalse(calibrator.isQualityScoresRequired());
2937         assertTrue(calibrator.isCommonAxisUsed());
2938         assertNull(calibrator.getListener());
2939         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
2940         assertFalse(calibrator.isReady());
2941         assertFalse(calibrator.isRunning());
2942         assertNull(calibrator.getEstimatedMa());
2943         assertNull(calibrator.getEstimatedSx());
2944         assertNull(calibrator.getEstimatedSy());
2945         assertNull(calibrator.getEstimatedSz());
2946         assertNull(calibrator.getEstimatedMxy());
2947         assertNull(calibrator.getEstimatedMxz());
2948         assertNull(calibrator.getEstimatedMyx());
2949         assertNull(calibrator.getEstimatedMyz());
2950         assertNull(calibrator.getEstimatedMzx());
2951         assertNull(calibrator.getEstimatedMzy());
2952         assertNull(calibrator.getEstimatedCovariance());
2953         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
2954         assertNull(calibrator.getGroundTruthGravityNorm());
2955         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2956         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2957         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
2958     }
2959 
2960     @Test
2961     void testConstructor31() throws WrongSizeException {
2962         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
2963         final var ba = generateBa();
2964         final var biasX = ba.getElementAtIndex(0);
2965         final var biasY = ba.getElementAtIndex(1);
2966         final var biasZ = ba.getElementAtIndex(2);
2967 
2968         final var ma = generateMaCommonAxis();
2969         final var sx = ma.getElementAt(0, 0);
2970         final var sy = ma.getElementAt(1, 1);
2971         final var sz = ma.getElementAt(2, 2);
2972 
2973         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
2974                 biasX, biasY, biasZ, sx, sy, sz, this);
2975 
2976         // check default values
2977         assertEquals(biasX, calibrator.getBiasX(), 0.0);
2978         assertEquals(biasY, calibrator.getBiasY(), 0.0);
2979         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
2980         final var bx1 = calibrator.getBiasXAsAcceleration();
2981         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
2982         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
2983         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2984         calibrator.getBiasXAsAcceleration(bx2);
2985         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
2986         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
2987         final var by1 = calibrator.getBiasYAsAcceleration();
2988         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
2989         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
2990         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2991         calibrator.getBiasYAsAcceleration(by2);
2992         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
2993         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
2994         final var bz1 = calibrator.getBiasZAsAcceleration();
2995         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
2996         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
2997         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
2998         calibrator.getBiasZAsAcceleration(bz2);
2999         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3000         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3001         final var biasTriad1 = calibrator.getBiasAsTriad();
3002         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3003         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3004         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3005         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3006         final var biasTriad2 = new AccelerationTriad();
3007         calibrator.getBiasAsTriad(biasTriad2);
3008         assertEquals(biasTriad1, biasTriad2);
3009         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3010         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3011         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3012         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3013         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3014         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3015         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3016         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3017         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3018         final var bias1 = calibrator.getBias();
3019         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3020         final var bias2 = new double[3];
3021         calibrator.getBias(bias2);
3022         assertArrayEquals(bias1, bias2, 0.0);
3023         final var b1 = calibrator.getBiasAsMatrix();
3024         assertEquals(b1, ba);
3025         final var b2 = new Matrix(3, 1);
3026         calibrator.getBiasAsMatrix(b2);
3027         assertEquals(b1, b2);
3028         final var ma1 = calibrator.getInitialMa();
3029         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3030         final var ma2 = new Matrix(3, 3);
3031         calibrator.getInitialMa(ma2);
3032         assertEquals(ma1, ma2);
3033         assertSame(measurements, calibrator.getMeasurements());
3034         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3035                 calibrator.getMeasurementType());
3036         assertFalse(calibrator.isOrderedMeasurementsRequired());
3037         assertFalse(calibrator.isQualityScoresRequired());
3038         assertTrue(calibrator.isCommonAxisUsed());
3039         assertSame(this, calibrator.getListener());
3040         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3041         assertFalse(calibrator.isReady());
3042         assertFalse(calibrator.isRunning());
3043         assertNull(calibrator.getEstimatedMa());
3044         assertNull(calibrator.getEstimatedSx());
3045         assertNull(calibrator.getEstimatedSy());
3046         assertNull(calibrator.getEstimatedSz());
3047         assertNull(calibrator.getEstimatedMxy());
3048         assertNull(calibrator.getEstimatedMxz());
3049         assertNull(calibrator.getEstimatedMyx());
3050         assertNull(calibrator.getEstimatedMyz());
3051         assertNull(calibrator.getEstimatedMzx());
3052         assertNull(calibrator.getEstimatedMzy());
3053         assertNull(calibrator.getEstimatedCovariance());
3054         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3055         assertNull(calibrator.getGroundTruthGravityNorm());
3056         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3057         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3058         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3059     }
3060 
3061     @Test
3062     void testConstructor32() throws WrongSizeException {
3063         final var ba = generateBa();
3064         final var biasX = ba.getElementAtIndex(0);
3065         final var biasY = ba.getElementAtIndex(1);
3066         final var biasZ = ba.getElementAtIndex(2);
3067 
3068         final var ma = generateMaCommonAxis();
3069         final var sx = ma.getElementAt(0, 0);
3070         final var sy = ma.getElementAt(1, 1);
3071         final var sz = ma.getElementAt(2, 2);
3072 
3073         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3074         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3075         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3076 
3077         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz);
3078 
3079         // check default values
3080         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3081         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3082         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3083         final var bx1 = calibrator.getBiasXAsAcceleration();
3084         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3085         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3086         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3087         calibrator.getBiasXAsAcceleration(bx2);
3088         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3089         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3090         final var by1 = calibrator.getBiasYAsAcceleration();
3091         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3092         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3093         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3094         calibrator.getBiasYAsAcceleration(by2);
3095         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3096         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3097         final var bz1 = calibrator.getBiasZAsAcceleration();
3098         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3099         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3100         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3101         calibrator.getBiasZAsAcceleration(bz2);
3102         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3103         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3104         final var biasTriad1 = calibrator.getBiasAsTriad();
3105         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3106         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3107         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3108         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3109         final var biasTriad2 = new AccelerationTriad();
3110         calibrator.getBiasAsTriad(biasTriad2);
3111         assertEquals(biasTriad1, biasTriad2);
3112         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3113         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3114         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3115         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3116         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3117         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3118         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3119         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3120         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3121         final var bias1 = calibrator.getBias();
3122         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3123         final var bias2 = new double[3];
3124         calibrator.getBias(bias2);
3125         assertArrayEquals(bias1, bias2, 0.0);
3126         final var b1 = calibrator.getBiasAsMatrix();
3127         assertEquals(b1, ba);
3128         final var b2 = new Matrix(3, 1);
3129         calibrator.getBiasAsMatrix(b2);
3130         assertEquals(b1, b2);
3131         final var ma1 = calibrator.getInitialMa();
3132         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3133         final var ma2 = new Matrix(3, 3);
3134         calibrator.getInitialMa(ma2);
3135         assertEquals(ma1, ma2);
3136         assertNull(calibrator.getMeasurements());
3137         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3138                 calibrator.getMeasurementType());
3139         assertFalse(calibrator.isOrderedMeasurementsRequired());
3140         assertFalse(calibrator.isQualityScoresRequired());
3141         assertFalse(calibrator.isCommonAxisUsed());
3142         assertNull(calibrator.getListener());
3143         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3144         assertFalse(calibrator.isReady());
3145         assertFalse(calibrator.isRunning());
3146         assertNull(calibrator.getEstimatedMa());
3147         assertNull(calibrator.getEstimatedSx());
3148         assertNull(calibrator.getEstimatedSy());
3149         assertNull(calibrator.getEstimatedSz());
3150         assertNull(calibrator.getEstimatedMxy());
3151         assertNull(calibrator.getEstimatedMxz());
3152         assertNull(calibrator.getEstimatedMyx());
3153         assertNull(calibrator.getEstimatedMyz());
3154         assertNull(calibrator.getEstimatedMzx());
3155         assertNull(calibrator.getEstimatedMzy());
3156         assertNull(calibrator.getEstimatedCovariance());
3157         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3158         assertNull(calibrator.getGroundTruthGravityNorm());
3159         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3160         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3161         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3162     }
3163 
3164     @Test
3165     void testConstructor33() throws WrongSizeException {
3166         final var ba = generateBa();
3167         final var biasX = ba.getElementAtIndex(0);
3168         final var biasY = ba.getElementAtIndex(1);
3169         final var biasZ = ba.getElementAtIndex(2);
3170 
3171         final var ma = generateMaCommonAxis();
3172         final var sx = ma.getElementAt(0, 0);
3173         final var sy = ma.getElementAt(1, 1);
3174         final var sz = ma.getElementAt(2, 2);
3175 
3176         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3177         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3178         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3179 
3180         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz, this);
3181 
3182         // check default values
3183         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3184         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3185         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3186         final var bx1 = calibrator.getBiasXAsAcceleration();
3187         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3188         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3189         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3190         calibrator.getBiasXAsAcceleration(bx2);
3191         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3192         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3193         final var by1 = calibrator.getBiasYAsAcceleration();
3194         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3195         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3196         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3197         calibrator.getBiasYAsAcceleration(by2);
3198         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3199         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3200         final var bz1 = calibrator.getBiasZAsAcceleration();
3201         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3202         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3203         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3204         calibrator.getBiasZAsAcceleration(bz2);
3205         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3206         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3207         final var biasTriad1 = calibrator.getBiasAsTriad();
3208         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3209         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3210         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3211         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3212         final var biasTriad2 = new AccelerationTriad();
3213         calibrator.getBiasAsTriad(biasTriad2);
3214         assertEquals(biasTriad1, biasTriad2);
3215         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3216         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3217         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3218         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3219         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3220         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3221         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3222         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3223         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3224         final var bias1 = calibrator.getBias();
3225         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3226         final var bias2 = new double[3];
3227         calibrator.getBias(bias2);
3228         assertArrayEquals(bias1, bias2, 0.0);
3229         final var b1 = calibrator.getBiasAsMatrix();
3230         assertEquals(b1, ba);
3231         final var b2 = new Matrix(3, 1);
3232         calibrator.getBiasAsMatrix(b2);
3233         assertEquals(b1, b2);
3234         final var ma1 = calibrator.getInitialMa();
3235         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3236         final var ma2 = new Matrix(3, 3);
3237         calibrator.getInitialMa(ma2);
3238         assertEquals(ma1, ma2);
3239         assertNull(calibrator.getMeasurements());
3240         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3241                 calibrator.getMeasurementType());
3242         assertFalse(calibrator.isOrderedMeasurementsRequired());
3243         assertFalse(calibrator.isQualityScoresRequired());
3244         assertFalse(calibrator.isCommonAxisUsed());
3245         assertSame(this, calibrator.getListener());
3246         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3247         assertFalse(calibrator.isReady());
3248         assertFalse(calibrator.isRunning());
3249         assertNull(calibrator.getEstimatedMa());
3250         assertNull(calibrator.getEstimatedSx());
3251         assertNull(calibrator.getEstimatedSy());
3252         assertNull(calibrator.getEstimatedSz());
3253         assertNull(calibrator.getEstimatedMxy());
3254         assertNull(calibrator.getEstimatedMxz());
3255         assertNull(calibrator.getEstimatedMyx());
3256         assertNull(calibrator.getEstimatedMyz());
3257         assertNull(calibrator.getEstimatedMzx());
3258         assertNull(calibrator.getEstimatedMzy());
3259         assertNull(calibrator.getEstimatedCovariance());
3260         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3261         assertNull(calibrator.getGroundTruthGravityNorm());
3262         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3263         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3264         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3265     }
3266 
3267     @Test
3268     void testConstructor34() throws WrongSizeException {
3269         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3270 
3271         final var ba = generateBa();
3272         final var biasX = ba.getElementAtIndex(0);
3273         final var biasY = ba.getElementAtIndex(1);
3274         final var biasZ = ba.getElementAtIndex(2);
3275 
3276         final var ma = generateMaCommonAxis();
3277         final var sx = ma.getElementAt(0, 0);
3278         final var sy = ma.getElementAt(1, 1);
3279         final var sz = ma.getElementAt(2, 2);
3280 
3281         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3282         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3283         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3284 
3285         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz);
3286 
3287         // check default values
3288         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3289         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3290         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3291         final var bx1 = calibrator.getBiasXAsAcceleration();
3292         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3293         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3294         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3295         calibrator.getBiasXAsAcceleration(bx2);
3296         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3297         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3298         final var by1 = calibrator.getBiasYAsAcceleration();
3299         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3300         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3301         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3302         calibrator.getBiasYAsAcceleration(by2);
3303         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3304         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3305         final var bz1 = calibrator.getBiasZAsAcceleration();
3306         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3307         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3308         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3309         calibrator.getBiasZAsAcceleration(bz2);
3310         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3311         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3312         final var biasTriad1 = calibrator.getBiasAsTriad();
3313         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3314         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3315         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3316         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3317         final var biasTriad2 = new AccelerationTriad();
3318         calibrator.getBiasAsTriad(biasTriad2);
3319         assertEquals(biasTriad1, biasTriad2);
3320         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3321         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3322         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3323         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3324         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3325         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3326         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3327         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3328         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3329         final var bias1 = calibrator.getBias();
3330         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3331         final var bias2 = new double[3];
3332         calibrator.getBias(bias2);
3333         assertArrayEquals(bias1, bias2, 0.0);
3334         final var b1 = calibrator.getBiasAsMatrix();
3335         assertEquals(b1, ba);
3336         final var b2 = new Matrix(3, 1);
3337         calibrator.getBiasAsMatrix(b2);
3338         assertEquals(b1, b2);
3339         final var ma1 = calibrator.getInitialMa();
3340         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3341         final var ma2 = new Matrix(3, 3);
3342         calibrator.getInitialMa(ma2);
3343         assertEquals(ma1, ma2);
3344         assertSame(measurements, calibrator.getMeasurements());
3345         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3346                 calibrator.getMeasurementType());
3347         assertFalse(calibrator.isOrderedMeasurementsRequired());
3348         assertFalse(calibrator.isQualityScoresRequired());
3349         assertFalse(calibrator.isCommonAxisUsed());
3350         assertNull(calibrator.getListener());
3351         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3352         assertFalse(calibrator.isReady());
3353         assertFalse(calibrator.isRunning());
3354         assertNull(calibrator.getEstimatedMa());
3355         assertNull(calibrator.getEstimatedSx());
3356         assertNull(calibrator.getEstimatedSy());
3357         assertNull(calibrator.getEstimatedSz());
3358         assertNull(calibrator.getEstimatedMxy());
3359         assertNull(calibrator.getEstimatedMxz());
3360         assertNull(calibrator.getEstimatedMyx());
3361         assertNull(calibrator.getEstimatedMyz());
3362         assertNull(calibrator.getEstimatedMzx());
3363         assertNull(calibrator.getEstimatedMzy());
3364         assertNull(calibrator.getEstimatedCovariance());
3365         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3366         assertNull(calibrator.getGroundTruthGravityNorm());
3367         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3368         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3369         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3370     }
3371 
3372     @Test
3373     void testConstructor35() throws WrongSizeException {
3374         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3375 
3376         final var ba = generateBa();
3377         final var biasX = ba.getElementAtIndex(0);
3378         final var biasY = ba.getElementAtIndex(1);
3379         final var biasZ = ba.getElementAtIndex(2);
3380 
3381         final var ma = generateMaCommonAxis();
3382         final var sx = ma.getElementAt(0, 0);
3383         final var sy = ma.getElementAt(1, 1);
3384         final var sz = ma.getElementAt(2, 2);
3385 
3386         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3387         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3388         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3389 
3390         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz,
3391                 this);
3392 
3393         // check default values
3394         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3395         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3396         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3397         final var bx1 = calibrator.getBiasXAsAcceleration();
3398         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3399         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3400         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3401         calibrator.getBiasXAsAcceleration(bx2);
3402         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3403         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3404         final var by1 = calibrator.getBiasYAsAcceleration();
3405         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3406         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3407         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3408         calibrator.getBiasYAsAcceleration(by2);
3409         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3410         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3411         final var bz1 = calibrator.getBiasZAsAcceleration();
3412         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3413         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3414         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3415         calibrator.getBiasZAsAcceleration(bz2);
3416         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3417         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3418         final var biasTriad1 = calibrator.getBiasAsTriad();
3419         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3420         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3421         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3422         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3423         final var biasTriad2 = new AccelerationTriad();
3424         calibrator.getBiasAsTriad(biasTriad2);
3425         assertEquals(biasTriad1, biasTriad2);
3426         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3427         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3428         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3429         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3430         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3431         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3432         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3433         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3434         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3435         final var bias1 = calibrator.getBias();
3436         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3437         final var bias2 = new double[3];
3438         calibrator.getBias(bias2);
3439         assertArrayEquals(bias1, bias2, 0.0);
3440         final var b1 = calibrator.getBiasAsMatrix();
3441         assertEquals(b1, ba);
3442         final var b2 = new Matrix(3, 1);
3443         calibrator.getBiasAsMatrix(b2);
3444         assertEquals(b1, b2);
3445         final var ma1 = calibrator.getInitialMa();
3446         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3447         final var ma2 = new Matrix(3, 3);
3448         calibrator.getInitialMa(ma2);
3449         assertEquals(ma1, ma2);
3450         assertSame(measurements, calibrator.getMeasurements());
3451         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3452                 calibrator.getMeasurementType());
3453         assertFalse(calibrator.isOrderedMeasurementsRequired());
3454         assertFalse(calibrator.isQualityScoresRequired());
3455         assertFalse(calibrator.isCommonAxisUsed());
3456         assertSame(this, calibrator.getListener());
3457         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3458         assertFalse(calibrator.isReady());
3459         assertFalse(calibrator.isRunning());
3460         assertNull(calibrator.getEstimatedMa());
3461         assertNull(calibrator.getEstimatedSx());
3462         assertNull(calibrator.getEstimatedSy());
3463         assertNull(calibrator.getEstimatedSz());
3464         assertNull(calibrator.getEstimatedMxy());
3465         assertNull(calibrator.getEstimatedMxz());
3466         assertNull(calibrator.getEstimatedMyx());
3467         assertNull(calibrator.getEstimatedMyz());
3468         assertNull(calibrator.getEstimatedMzx());
3469         assertNull(calibrator.getEstimatedMzy());
3470         assertNull(calibrator.getEstimatedCovariance());
3471         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3472         assertNull(calibrator.getGroundTruthGravityNorm());
3473         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3474         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3475         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3476     }
3477 
3478     @Test
3479     void testConstructor36() throws WrongSizeException {
3480         final var ba = generateBa();
3481         final var biasX = ba.getElementAtIndex(0);
3482         final var biasY = ba.getElementAtIndex(1);
3483         final var biasZ = ba.getElementAtIndex(2);
3484 
3485         final var ma = generateMaCommonAxis();
3486         final var sx = ma.getElementAt(0, 0);
3487         final var sy = ma.getElementAt(1, 1);
3488         final var sz = ma.getElementAt(2, 2);
3489 
3490         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3491         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3492         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3493 
3494         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
3495                 sx, sy, sz);
3496 
3497         // check default values
3498         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3499         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3500         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3501         final var bx1 = calibrator.getBiasXAsAcceleration();
3502         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3503         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3504         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3505         calibrator.getBiasXAsAcceleration(bx2);
3506         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3507         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3508         final var by1 = calibrator.getBiasYAsAcceleration();
3509         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3510         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3511         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3512         calibrator.getBiasYAsAcceleration(by2);
3513         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3514         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3515         final var bz1 = calibrator.getBiasZAsAcceleration();
3516         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3517         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3518         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3519         calibrator.getBiasZAsAcceleration(bz2);
3520         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3521         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3522         final var biasTriad1 = calibrator.getBiasAsTriad();
3523         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3524         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3525         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3526         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3527         final var biasTriad2 = new AccelerationTriad();
3528         calibrator.getBiasAsTriad(biasTriad2);
3529         assertEquals(biasTriad1, biasTriad2);
3530         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3531         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3532         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3533         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3534         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3535         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3536         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3537         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3538         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3539         final var bias1 = calibrator.getBias();
3540         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3541         final var bias2 = new double[3];
3542         calibrator.getBias(bias2);
3543         assertArrayEquals(bias1, bias2, 0.0);
3544         final var b1 = calibrator.getBiasAsMatrix();
3545         assertEquals(b1, ba);
3546         final var b2 = new Matrix(3, 1);
3547         calibrator.getBiasAsMatrix(b2);
3548         assertEquals(b1, b2);
3549         final var ma1 = calibrator.getInitialMa();
3550         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3551         final var ma2 = new Matrix(3, 3);
3552         calibrator.getInitialMa(ma2);
3553         assertEquals(ma1, ma2);
3554         assertNull(calibrator.getMeasurements());
3555         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3556                 calibrator.getMeasurementType());
3557         assertFalse(calibrator.isOrderedMeasurementsRequired());
3558         assertFalse(calibrator.isQualityScoresRequired());
3559         assertTrue(calibrator.isCommonAxisUsed());
3560         assertNull(calibrator.getListener());
3561         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3562         assertFalse(calibrator.isReady());
3563         assertFalse(calibrator.isRunning());
3564         assertNull(calibrator.getEstimatedMa());
3565         assertNull(calibrator.getEstimatedSx());
3566         assertNull(calibrator.getEstimatedSy());
3567         assertNull(calibrator.getEstimatedSz());
3568         assertNull(calibrator.getEstimatedMxy());
3569         assertNull(calibrator.getEstimatedMxz());
3570         assertNull(calibrator.getEstimatedMyx());
3571         assertNull(calibrator.getEstimatedMyz());
3572         assertNull(calibrator.getEstimatedMzx());
3573         assertNull(calibrator.getEstimatedMzy());
3574         assertNull(calibrator.getEstimatedCovariance());
3575         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3576         assertNull(calibrator.getGroundTruthGravityNorm());
3577         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3578         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3579         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3580     }
3581 
3582     @Test
3583     void testConstructor37() throws WrongSizeException {
3584         final var ba = generateBa();
3585         final var biasX = ba.getElementAtIndex(0);
3586         final var biasY = ba.getElementAtIndex(1);
3587         final var biasZ = ba.getElementAtIndex(2);
3588 
3589         final var ma = generateMaCommonAxis();
3590         final var sx = ma.getElementAt(0, 0);
3591         final var sy = ma.getElementAt(1, 1);
3592         final var sz = ma.getElementAt(2, 2);
3593 
3594         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3595         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3596         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3597 
3598         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
3599                 sx, sy, sz, this);
3600 
3601         // check default values
3602         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3603         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3604         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3605         final var bx1 = calibrator.getBiasXAsAcceleration();
3606         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3607         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3608         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3609         calibrator.getBiasXAsAcceleration(bx2);
3610         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3611         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3612         final var by1 = calibrator.getBiasYAsAcceleration();
3613         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3614         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3615         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3616         calibrator.getBiasYAsAcceleration(by2);
3617         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3618         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3619         final var bz1 = calibrator.getBiasZAsAcceleration();
3620         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3621         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3622         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3623         calibrator.getBiasZAsAcceleration(bz2);
3624         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3625         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3626         final var biasTriad1 = calibrator.getBiasAsTriad();
3627         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3628         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3629         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3630         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3631         final var biasTriad2 = new AccelerationTriad();
3632         calibrator.getBiasAsTriad(biasTriad2);
3633         assertEquals(biasTriad1, biasTriad2);
3634         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3635         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3636         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3637         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3638         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3639         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3640         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3641         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3642         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3643         final var bias1 = calibrator.getBias();
3644         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3645         final var bias2 = new double[3];
3646         calibrator.getBias(bias2);
3647         assertArrayEquals(bias1, bias2, 0.0);
3648         final var b1 = calibrator.getBiasAsMatrix();
3649         assertEquals(b1, ba);
3650         final var b2 = new Matrix(3, 1);
3651         calibrator.getBiasAsMatrix(b2);
3652         assertEquals(b1, b2);
3653         final var ma1 = calibrator.getInitialMa();
3654         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3655         final var ma2 = new Matrix(3, 3);
3656         calibrator.getInitialMa(ma2);
3657         assertEquals(ma1, ma2);
3658         assertNull(calibrator.getMeasurements());
3659         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3660                 calibrator.getMeasurementType());
3661         assertFalse(calibrator.isOrderedMeasurementsRequired());
3662         assertFalse(calibrator.isQualityScoresRequired());
3663         assertTrue(calibrator.isCommonAxisUsed());
3664         assertSame(this, calibrator.getListener());
3665         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3666         assertFalse(calibrator.isReady());
3667         assertFalse(calibrator.isRunning());
3668         assertNull(calibrator.getEstimatedMa());
3669         assertNull(calibrator.getEstimatedSx());
3670         assertNull(calibrator.getEstimatedSy());
3671         assertNull(calibrator.getEstimatedSz());
3672         assertNull(calibrator.getEstimatedMxy());
3673         assertNull(calibrator.getEstimatedMxz());
3674         assertNull(calibrator.getEstimatedMyx());
3675         assertNull(calibrator.getEstimatedMyz());
3676         assertNull(calibrator.getEstimatedMzx());
3677         assertNull(calibrator.getEstimatedMzy());
3678         assertNull(calibrator.getEstimatedCovariance());
3679         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3680         assertNull(calibrator.getGroundTruthGravityNorm());
3681         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3682         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3683         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3684     }
3685 
3686     @Test
3687     void testConstructor38() throws WrongSizeException {
3688         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3689 
3690         final var ba = generateBa();
3691         final var biasX = ba.getElementAtIndex(0);
3692         final var biasY = ba.getElementAtIndex(1);
3693         final var biasZ = ba.getElementAtIndex(2);
3694 
3695         final var ma = generateMaCommonAxis();
3696         final var sx = ma.getElementAt(0, 0);
3697         final var sy = ma.getElementAt(1, 1);
3698         final var sz = ma.getElementAt(2, 2);
3699 
3700         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3701         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3702         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3703 
3704         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
3705                 bx, by, bz, sx, sy, sz);
3706 
3707         // check default values
3708         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3709         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3710         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3711         final var bx1 = calibrator.getBiasXAsAcceleration();
3712         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3713         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3714         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3715         calibrator.getBiasXAsAcceleration(bx2);
3716         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3717         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3718         final var by1 = calibrator.getBiasYAsAcceleration();
3719         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3720         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3721         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3722         calibrator.getBiasYAsAcceleration(by2);
3723         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3724         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3725         final var bz1 = calibrator.getBiasZAsAcceleration();
3726         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3727         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3728         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3729         calibrator.getBiasZAsAcceleration(bz2);
3730         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3731         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3732         final var biasTriad1 = calibrator.getBiasAsTriad();
3733         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3734         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3735         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3736         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3737         final var biasTriad2 = new AccelerationTriad();
3738         calibrator.getBiasAsTriad(biasTriad2);
3739         assertEquals(biasTriad1, biasTriad2);
3740         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3741         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3742         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3743         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3744         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3745         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3746         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3747         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3748         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3749         final var bias1 = calibrator.getBias();
3750         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3751         final var bias2 = new double[3];
3752         calibrator.getBias(bias2);
3753         assertArrayEquals(bias1, bias2, 0.0);
3754         final var b1 = calibrator.getBiasAsMatrix();
3755         assertEquals(b1, ba);
3756         final var b2 = new Matrix(3, 1);
3757         calibrator.getBiasAsMatrix(b2);
3758         assertEquals(b1, b2);
3759         final var ma1 = calibrator.getInitialMa();
3760         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3761         final var ma2 = new Matrix(3, 3);
3762         calibrator.getInitialMa(ma2);
3763         assertEquals(ma1, ma2);
3764         assertSame(measurements, calibrator.getMeasurements());
3765         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3766                 calibrator.getMeasurementType());
3767         assertFalse(calibrator.isOrderedMeasurementsRequired());
3768         assertFalse(calibrator.isQualityScoresRequired());
3769         assertTrue(calibrator.isCommonAxisUsed());
3770         assertNull(calibrator.getListener());
3771         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3772         assertFalse(calibrator.isReady());
3773         assertFalse(calibrator.isRunning());
3774         assertNull(calibrator.getEstimatedMa());
3775         assertNull(calibrator.getEstimatedSx());
3776         assertNull(calibrator.getEstimatedSy());
3777         assertNull(calibrator.getEstimatedSz());
3778         assertNull(calibrator.getEstimatedMxy());
3779         assertNull(calibrator.getEstimatedMxz());
3780         assertNull(calibrator.getEstimatedMyx());
3781         assertNull(calibrator.getEstimatedMyz());
3782         assertNull(calibrator.getEstimatedMzx());
3783         assertNull(calibrator.getEstimatedMzy());
3784         assertNull(calibrator.getEstimatedCovariance());
3785         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3786         assertNull(calibrator.getGroundTruthGravityNorm());
3787         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3788         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3789         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3790     }
3791 
3792     @Test
3793     void testConstructor39() throws WrongSizeException {
3794         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
3795 
3796         final var ba = generateBa();
3797         final var biasX = ba.getElementAtIndex(0);
3798         final var biasY = ba.getElementAtIndex(1);
3799         final var biasZ = ba.getElementAtIndex(2);
3800 
3801         final var ma = generateMaCommonAxis();
3802         final var sx = ma.getElementAt(0, 0);
3803         final var sy = ma.getElementAt(1, 1);
3804         final var sz = ma.getElementAt(2, 2);
3805 
3806         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3807         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3808         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
3809 
3810         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
3811                 bx, by, bz, sx, sy, sz, this);
3812 
3813         // check default values
3814         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3815         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3816         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3817         final var bx1 = calibrator.getBiasXAsAcceleration();
3818         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3819         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3820         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3821         calibrator.getBiasXAsAcceleration(bx2);
3822         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3823         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3824         final var by1 = calibrator.getBiasYAsAcceleration();
3825         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3826         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3827         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3828         calibrator.getBiasYAsAcceleration(by2);
3829         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3830         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3831         final var bz1 = calibrator.getBiasZAsAcceleration();
3832         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3833         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3834         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3835         calibrator.getBiasZAsAcceleration(bz2);
3836         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3837         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3838         final var biasTriad1 = calibrator.getBiasAsTriad();
3839         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3840         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3841         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3842         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3843         final var biasTriad2 = new AccelerationTriad();
3844         calibrator.getBiasAsTriad(biasTriad2);
3845         assertEquals(biasTriad1, biasTriad2);
3846         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3847         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3848         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3849         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
3850         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
3851         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
3852         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
3853         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
3854         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
3855         final var bias1 = calibrator.getBias();
3856         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3857         final var bias2 = new double[3];
3858         calibrator.getBias(bias2);
3859         assertArrayEquals(bias1, bias2, 0.0);
3860         final var b1 = calibrator.getBiasAsMatrix();
3861         assertEquals(b1, ba);
3862         final var b2 = new Matrix(3, 1);
3863         calibrator.getBiasAsMatrix(b2);
3864         assertEquals(b1, b2);
3865         final var ma1 = calibrator.getInitialMa();
3866         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3867         final var ma2 = new Matrix(3, 3);
3868         calibrator.getInitialMa(ma2);
3869         assertEquals(ma1, ma2);
3870         assertSame(measurements, calibrator.getMeasurements());
3871         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3872                 calibrator.getMeasurementType());
3873         assertFalse(calibrator.isOrderedMeasurementsRequired());
3874         assertFalse(calibrator.isQualityScoresRequired());
3875         assertTrue(calibrator.isCommonAxisUsed());
3876         assertSame(this, calibrator.getListener());
3877         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
3878         assertFalse(calibrator.isReady());
3879         assertFalse(calibrator.isRunning());
3880         assertNull(calibrator.getEstimatedMa());
3881         assertNull(calibrator.getEstimatedSx());
3882         assertNull(calibrator.getEstimatedSy());
3883         assertNull(calibrator.getEstimatedSz());
3884         assertNull(calibrator.getEstimatedMxy());
3885         assertNull(calibrator.getEstimatedMxz());
3886         assertNull(calibrator.getEstimatedMyx());
3887         assertNull(calibrator.getEstimatedMyz());
3888         assertNull(calibrator.getEstimatedMzx());
3889         assertNull(calibrator.getEstimatedMzy());
3890         assertNull(calibrator.getEstimatedCovariance());
3891         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
3892         assertNull(calibrator.getGroundTruthGravityNorm());
3893         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3894         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3895         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
3896     }
3897 
3898     @Test
3899     void testConstructor40() throws WrongSizeException {
3900         final var ba = generateBa();
3901         final var biasX = ba.getElementAtIndex(0);
3902         final var biasY = ba.getElementAtIndex(1);
3903         final var biasZ = ba.getElementAtIndex(2);
3904 
3905         final var ma = generateMaCommonAxis();
3906         final var sx = ma.getElementAt(0, 0);
3907         final var sy = ma.getElementAt(1, 1);
3908         final var sz = ma.getElementAt(2, 2);
3909         final var mxy = ma.getElementAt(0, 1);
3910         final var mxz = ma.getElementAt(0, 2);
3911         final var myx = ma.getElementAt(1, 0);
3912         final var myz = ma.getElementAt(1, 2);
3913         final var mzx = ma.getElementAt(2, 0);
3914         final var mzy = ma.getElementAt(2, 1);
3915 
3916         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(biasX, biasY, biasZ, sx, sy, sz,
3917                 mxy, mxz, myx, myz, mzx, mzy);
3918 
3919         // check default values
3920         assertEquals(biasX, calibrator.getBiasX(), 0.0);
3921         assertEquals(biasY, calibrator.getBiasY(), 0.0);
3922         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
3923         final var bx1 = calibrator.getBiasXAsAcceleration();
3924         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
3925         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
3926         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3927         calibrator.getBiasXAsAcceleration(bx2);
3928         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
3929         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
3930         final var by1 = calibrator.getBiasYAsAcceleration();
3931         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
3932         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
3933         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3934         calibrator.getBiasYAsAcceleration(by2);
3935         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
3936         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
3937         final var bz1 = calibrator.getBiasZAsAcceleration();
3938         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
3939         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
3940         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
3941         calibrator.getBiasZAsAcceleration(bz2);
3942         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
3943         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
3944         final var biasTriad1 = calibrator.getBiasAsTriad();
3945         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
3946         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
3947         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
3948         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
3949         final var biasTriad2 = new AccelerationTriad();
3950         calibrator.getBiasAsTriad(biasTriad2);
3951         assertEquals(biasTriad1, biasTriad2);
3952         assertEquals(sx, calibrator.getInitialSx(), 0.0);
3953         assertEquals(sy, calibrator.getInitialSy(), 0.0);
3954         assertEquals(sz, calibrator.getInitialSz(), 0.0);
3955         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
3956         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
3957         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
3958         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
3959         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
3960         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
3961         final var bias1 = calibrator.getBias();
3962         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3963         final var bias2 = new double[3];
3964         calibrator.getBias(bias2);
3965         assertArrayEquals(bias1, bias2, 0.0);
3966         final var b1 = calibrator.getBiasAsMatrix();
3967         assertEquals(b1, ba);
3968         final var b2 = new Matrix(3, 1);
3969         calibrator.getBiasAsMatrix(b2);
3970         assertEquals(b1, b2);
3971         final var ma1 = new Matrix(3, 3);
3972         ma1.setSubmatrix(0, 0, 2, 2,
3973                 new double[]{sx, myx, mzx,
3974                         mxy, sy, mzy,
3975                         mxz, myz, sz});
3976         assertEquals(calibrator.getInitialMa(), ma1);
3977         final var ma2 = new Matrix(3, 3);
3978         calibrator.getInitialMa(ma2);
3979         assertEquals(ma1, ma2);
3980         assertNull(calibrator.getMeasurements());
3981         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
3982                 calibrator.getMeasurementType());
3983         assertFalse(calibrator.isOrderedMeasurementsRequired());
3984         assertFalse(calibrator.isQualityScoresRequired());
3985         assertFalse(calibrator.isCommonAxisUsed());
3986         assertNull(calibrator.getListener());
3987         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
3988         assertFalse(calibrator.isReady());
3989         assertFalse(calibrator.isRunning());
3990         assertNull(calibrator.getEstimatedMa());
3991         assertNull(calibrator.getEstimatedSx());
3992         assertNull(calibrator.getEstimatedSy());
3993         assertNull(calibrator.getEstimatedSz());
3994         assertNull(calibrator.getEstimatedMxy());
3995         assertNull(calibrator.getEstimatedMxz());
3996         assertNull(calibrator.getEstimatedMyx());
3997         assertNull(calibrator.getEstimatedMyz());
3998         assertNull(calibrator.getEstimatedMzx());
3999         assertNull(calibrator.getEstimatedMzy());
4000         assertNull(calibrator.getEstimatedCovariance());
4001         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4002         assertNull(calibrator.getGroundTruthGravityNorm());
4003         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4004         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4005         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4006     }
4007 
4008     @Test
4009     void testConstructor41() throws WrongSizeException {
4010         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4011 
4012         final var ba = generateBa();
4013         final var biasX = ba.getElementAtIndex(0);
4014         final var biasY = ba.getElementAtIndex(1);
4015         final var biasZ = ba.getElementAtIndex(2);
4016 
4017         final var ma = generateMaCommonAxis();
4018         final var sx = ma.getElementAt(0, 0);
4019         final var sy = ma.getElementAt(1, 1);
4020         final var sz = ma.getElementAt(2, 2);
4021         final var mxy = ma.getElementAt(0, 1);
4022         final var mxz = ma.getElementAt(0, 2);
4023         final var myx = ma.getElementAt(1, 0);
4024         final var myz = ma.getElementAt(1, 2);
4025         final var mzx = ma.getElementAt(2, 0);
4026         final var mzy = ma.getElementAt(2, 1);
4027 
4028         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
4029                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4030 
4031         // check default values
4032         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4033         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4034         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4035         final var bx1 = calibrator.getBiasXAsAcceleration();
4036         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4037         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4038         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4039         calibrator.getBiasXAsAcceleration(bx2);
4040         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4041         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4042         final var by1 = calibrator.getBiasYAsAcceleration();
4043         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4044         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4045         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4046         calibrator.getBiasYAsAcceleration(by2);
4047         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4048         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4049         final var bz1 = calibrator.getBiasZAsAcceleration();
4050         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4051         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4052         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4053         calibrator.getBiasZAsAcceleration(bz2);
4054         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4055         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4056         final var biasTriad1 = calibrator.getBiasAsTriad();
4057         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4058         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4059         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4060         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4061         final var biasTriad2 = new AccelerationTriad();
4062         calibrator.getBiasAsTriad(biasTriad2);
4063         assertEquals(biasTriad1, biasTriad2);
4064         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4065         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4066         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4067         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4068         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4069         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4070         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4071         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4072         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4073         final var bias1 = calibrator.getBias();
4074         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4075         final var bias2 = new double[3];
4076         calibrator.getBias(bias2);
4077         assertArrayEquals(bias1, bias2, 0.0);
4078         final var b1 = calibrator.getBiasAsMatrix();
4079         assertEquals(b1, ba);
4080         final var b2 = new Matrix(3, 1);
4081         calibrator.getBiasAsMatrix(b2);
4082         assertEquals(b1, b2);
4083         final var ma1 = new Matrix(3, 3);
4084         ma1.setSubmatrix(0, 0, 2, 2,
4085                 new double[]{sx, myx, mzx,
4086                         mxy, sy, mzy,
4087                         mxz, myz, sz});
4088         assertEquals(ma1, calibrator.getInitialMa());
4089         final var ma2 = new Matrix(3, 3);
4090         calibrator.getInitialMa(ma2);
4091         assertEquals(ma1, ma2);
4092         assertSame(measurements, calibrator.getMeasurements());
4093         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4094                 calibrator.getMeasurementType());
4095         assertFalse(calibrator.isOrderedMeasurementsRequired());
4096         assertFalse(calibrator.isQualityScoresRequired());
4097         assertFalse(calibrator.isCommonAxisUsed());
4098         assertNull(calibrator.getListener());
4099         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4100         assertFalse(calibrator.isReady());
4101         assertFalse(calibrator.isRunning());
4102         assertNull(calibrator.getEstimatedMa());
4103         assertNull(calibrator.getEstimatedSx());
4104         assertNull(calibrator.getEstimatedSy());
4105         assertNull(calibrator.getEstimatedSz());
4106         assertNull(calibrator.getEstimatedMxy());
4107         assertNull(calibrator.getEstimatedMxz());
4108         assertNull(calibrator.getEstimatedMyx());
4109         assertNull(calibrator.getEstimatedMyz());
4110         assertNull(calibrator.getEstimatedMzx());
4111         assertNull(calibrator.getEstimatedMzy());
4112         assertNull(calibrator.getEstimatedCovariance());
4113         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4114         assertNull(calibrator.getGroundTruthGravityNorm());
4115         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4116         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4117         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4118     }
4119 
4120     @Test
4121     void testConstructor42() throws WrongSizeException {
4122         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4123 
4124         final var ba = generateBa();
4125         final var biasX = ba.getElementAtIndex(0);
4126         final var biasY = ba.getElementAtIndex(1);
4127         final var biasZ = ba.getElementAtIndex(2);
4128 
4129         final var ma = generateMaCommonAxis();
4130         final var sx = ma.getElementAt(0, 0);
4131         final var sy = ma.getElementAt(1, 1);
4132         final var sz = ma.getElementAt(2, 2);
4133         final var mxy = ma.getElementAt(0, 1);
4134         final var mxz = ma.getElementAt(0, 2);
4135         final var myx = ma.getElementAt(1, 0);
4136         final var myz = ma.getElementAt(1, 2);
4137         final var mzx = ma.getElementAt(2, 0);
4138         final var mzy = ma.getElementAt(2, 1);
4139 
4140         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, biasX, biasY, biasZ,
4141                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4142 
4143         // check default values
4144         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4145         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4146         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4147         final var bx1 = calibrator.getBiasXAsAcceleration();
4148         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4149         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4150         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4151         calibrator.getBiasXAsAcceleration(bx2);
4152         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4153         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4154         final var by1 = calibrator.getBiasYAsAcceleration();
4155         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4156         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4157         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4158         calibrator.getBiasYAsAcceleration(by2);
4159         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4160         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4161         final var bz1 = calibrator.getBiasZAsAcceleration();
4162         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4163         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4164         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4165         calibrator.getBiasZAsAcceleration(bz2);
4166         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4167         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4168         final var biasTriad1 = calibrator.getBiasAsTriad();
4169         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4170         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4171         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4172         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4173         final var biasTriad2 = new AccelerationTriad();
4174         calibrator.getBiasAsTriad(biasTriad2);
4175         assertEquals(biasTriad1, biasTriad2);
4176         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4177         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4178         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4179         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4180         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4181         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4182         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4183         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4184         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4185         final var bias1 = calibrator.getBias();
4186         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4187         final var bias2 = new double[3];
4188         calibrator.getBias(bias2);
4189         assertArrayEquals(bias1, bias2, 0.0);
4190         final var b1 = calibrator.getBiasAsMatrix();
4191         assertEquals(b1, ba);
4192         final var b2 = new Matrix(3, 1);
4193         calibrator.getBiasAsMatrix(b2);
4194         assertEquals(b1, b2);
4195         final var ma1 = new Matrix(3, 3);
4196         ma1.setSubmatrix(0, 0, 2, 2,
4197                 new double[]{sx, myx, mzx,
4198                         mxy, sy, mzy,
4199                         mxz, myz, sz});
4200         assertEquals(calibrator.getInitialMa(), ma1);
4201         final var ma2 = new Matrix(3, 3);
4202         calibrator.getInitialMa(ma2);
4203         assertEquals(ma1, ma2);
4204         assertSame(calibrator.getMeasurements(), measurements);
4205         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4206                 calibrator.getMeasurementType());
4207         assertFalse(calibrator.isOrderedMeasurementsRequired());
4208         assertFalse(calibrator.isQualityScoresRequired());
4209         assertFalse(calibrator.isCommonAxisUsed());
4210         assertSame(this, calibrator.getListener());
4211         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4212         assertFalse(calibrator.isReady());
4213         assertFalse(calibrator.isRunning());
4214         assertNull(calibrator.getEstimatedMa());
4215         assertNull(calibrator.getEstimatedSx());
4216         assertNull(calibrator.getEstimatedSy());
4217         assertNull(calibrator.getEstimatedSz());
4218         assertNull(calibrator.getEstimatedMxy());
4219         assertNull(calibrator.getEstimatedMxz());
4220         assertNull(calibrator.getEstimatedMyx());
4221         assertNull(calibrator.getEstimatedMyz());
4222         assertNull(calibrator.getEstimatedMzx());
4223         assertNull(calibrator.getEstimatedMzy());
4224         assertNull(calibrator.getEstimatedCovariance());
4225         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4226         assertNull(calibrator.getGroundTruthGravityNorm());
4227         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4228         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4229         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4230     }
4231 
4232     @Test
4233     void testConstructor43() throws WrongSizeException {
4234         final var ba = generateBa();
4235         final var biasX = ba.getElementAtIndex(0);
4236         final var biasY = ba.getElementAtIndex(1);
4237         final var biasZ = ba.getElementAtIndex(2);
4238 
4239         final var ma = generateMaCommonAxis();
4240         final var sx = ma.getElementAt(0, 0);
4241         final var sy = ma.getElementAt(1, 1);
4242         final var sz = ma.getElementAt(2, 2);
4243         final var mxy = ma.getElementAt(0, 1);
4244         final var mxz = ma.getElementAt(0, 2);
4245         final var myx = ma.getElementAt(1, 0);
4246         final var myz = ma.getElementAt(1, 2);
4247         final var mzx = ma.getElementAt(2, 0);
4248         final var mzy = ma.getElementAt(2, 1);
4249 
4250         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
4251                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4252 
4253         // check default values
4254         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4255         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4256         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4257         final var bx1 = calibrator.getBiasXAsAcceleration();
4258         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4259         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4260         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4261         calibrator.getBiasXAsAcceleration(bx2);
4262         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4263         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4264         final var by1 = calibrator.getBiasYAsAcceleration();
4265         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4266         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4267         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4268         calibrator.getBiasYAsAcceleration(by2);
4269         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4270         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4271         final var bz1 = calibrator.getBiasZAsAcceleration();
4272         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4273         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4274         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4275         calibrator.getBiasZAsAcceleration(bz2);
4276         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4277         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4278         final var biasTriad1 = calibrator.getBiasAsTriad();
4279         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4280         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4281         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4282         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4283         final var biasTriad2 = new AccelerationTriad();
4284         calibrator.getBiasAsTriad(biasTriad2);
4285         assertEquals(biasTriad1, biasTriad2);
4286         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4287         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4288         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4289         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4290         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4291         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4292         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4293         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4294         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4295         final var bias1 = calibrator.getBias();
4296         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4297         final var bias2 = new double[3];
4298         calibrator.getBias(bias2);
4299         assertArrayEquals(bias1, bias2, 0.0);
4300         final var b1 = calibrator.getBiasAsMatrix();
4301         assertEquals(b1, ba);
4302         final var b2 = new Matrix(3, 1);
4303         calibrator.getBiasAsMatrix(b2);
4304         assertEquals(b1, b2);
4305         final var ma1 = new Matrix(3, 3);
4306         ma1.setSubmatrix(0, 0, 2, 2,
4307                 new double[]{sx, myx, mzx,
4308                         mxy, sy, mzy,
4309                         mxz, myz, sz});
4310         assertEquals(ma1, calibrator.getInitialMa());
4311         final var ma2 = new Matrix(3, 3);
4312         calibrator.getInitialMa(ma2);
4313         assertEquals(ma1, ma2);
4314         assertNull(calibrator.getMeasurements());
4315         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4316                 calibrator.getMeasurementType());
4317         assertFalse(calibrator.isOrderedMeasurementsRequired());
4318         assertFalse(calibrator.isQualityScoresRequired());
4319         assertTrue(calibrator.isCommonAxisUsed());
4320         assertNull(calibrator.getListener());
4321         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4322         assertFalse(calibrator.isReady());
4323         assertFalse(calibrator.isRunning());
4324         assertNull(calibrator.getEstimatedMa());
4325         assertNull(calibrator.getEstimatedSx());
4326         assertNull(calibrator.getEstimatedSy());
4327         assertNull(calibrator.getEstimatedSz());
4328         assertNull(calibrator.getEstimatedMxy());
4329         assertNull(calibrator.getEstimatedMxz());
4330         assertNull(calibrator.getEstimatedMyx());
4331         assertNull(calibrator.getEstimatedMyz());
4332         assertNull(calibrator.getEstimatedMzx());
4333         assertNull(calibrator.getEstimatedMzy());
4334         assertNull(calibrator.getEstimatedCovariance());
4335         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4336         assertNull(calibrator.getGroundTruthGravityNorm());
4337         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4338         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4339         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4340     }
4341 
4342     @Test
4343     void testConstructor44() throws WrongSizeException {
4344         final var ba = generateBa();
4345         final var biasX = ba.getElementAtIndex(0);
4346         final var biasY = ba.getElementAtIndex(1);
4347         final var biasZ = ba.getElementAtIndex(2);
4348 
4349         final var ma = generateMaCommonAxis();
4350         final var sx = ma.getElementAt(0, 0);
4351         final var sy = ma.getElementAt(1, 1);
4352         final var sz = ma.getElementAt(2, 2);
4353         final var mxy = ma.getElementAt(0, 1);
4354         final var mxz = ma.getElementAt(0, 2);
4355         final var myx = ma.getElementAt(1, 0);
4356         final var myz = ma.getElementAt(1, 2);
4357         final var mzx = ma.getElementAt(2, 0);
4358         final var mzy = ma.getElementAt(2, 1);
4359 
4360         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true,
4361                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4362 
4363         // check default values
4364         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4365         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4366         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4367         final var bx1 = calibrator.getBiasXAsAcceleration();
4368         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4369         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4370         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4371         calibrator.getBiasXAsAcceleration(bx2);
4372         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4373         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4374         final var by1 = calibrator.getBiasYAsAcceleration();
4375         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4376         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4377         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4378         calibrator.getBiasYAsAcceleration(by2);
4379         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4380         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4381         final var bz1 = calibrator.getBiasZAsAcceleration();
4382         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4383         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4384         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4385         calibrator.getBiasZAsAcceleration(bz2);
4386         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4387         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4388         final var biasTriad1 = calibrator.getBiasAsTriad();
4389         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4390         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4391         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4392         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4393         final var biasTriad2 = new AccelerationTriad();
4394         calibrator.getBiasAsTriad(biasTriad2);
4395         assertEquals(biasTriad1, biasTriad2);
4396         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4397         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4398         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4399         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4400         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4401         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4402         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4403         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4404         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4405         final var bias1 = calibrator.getBias();
4406         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4407         final var bias2 = new double[3];
4408         calibrator.getBias(bias2);
4409         assertArrayEquals(bias1, bias2, 0.0);
4410         final var b1 = calibrator.getBiasAsMatrix();
4411         assertEquals(b1, ba);
4412         final var b2 = new Matrix(3, 1);
4413         calibrator.getBiasAsMatrix(b2);
4414         assertEquals(b1, b2);
4415         final var ma1 = new Matrix(3, 3);
4416         ma1.setSubmatrix(0, 0, 2, 2,
4417                 new double[]{sx, myx, mzx,
4418                         mxy, sy, mzy,
4419                         mxz, myz, sz});
4420         assertEquals(ma1, calibrator.getInitialMa());
4421         final var ma2 = new Matrix(3, 3);
4422         calibrator.getInitialMa(ma2);
4423         assertEquals(ma1, ma2);
4424         assertNull(calibrator.getMeasurements());
4425         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4426                 calibrator.getMeasurementType());
4427         assertFalse(calibrator.isOrderedMeasurementsRequired());
4428         assertFalse(calibrator.isQualityScoresRequired());
4429         assertTrue(calibrator.isCommonAxisUsed());
4430         assertSame(this, calibrator.getListener());
4431         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4432         assertFalse(calibrator.isReady());
4433         assertFalse(calibrator.isRunning());
4434         assertNull(calibrator.getEstimatedMa());
4435         assertNull(calibrator.getEstimatedSx());
4436         assertNull(calibrator.getEstimatedSy());
4437         assertNull(calibrator.getEstimatedSz());
4438         assertNull(calibrator.getEstimatedMxy());
4439         assertNull(calibrator.getEstimatedMxz());
4440         assertNull(calibrator.getEstimatedMyx());
4441         assertNull(calibrator.getEstimatedMyz());
4442         assertNull(calibrator.getEstimatedMzx());
4443         assertNull(calibrator.getEstimatedMzy());
4444         assertNull(calibrator.getEstimatedCovariance());
4445         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4446         assertNull(calibrator.getGroundTruthGravityNorm());
4447         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4448         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4449         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4450     }
4451 
4452     @Test
4453     void testConstructor45() throws WrongSizeException {
4454         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4455 
4456         final var ba = generateBa();
4457         final var biasX = ba.getElementAtIndex(0);
4458         final var biasY = ba.getElementAtIndex(1);
4459         final var biasZ = ba.getElementAtIndex(2);
4460 
4461         final var ma = generateMaCommonAxis();
4462         final var sx = ma.getElementAt(0, 0);
4463         final var sy = ma.getElementAt(1, 1);
4464         final var sz = ma.getElementAt(2, 2);
4465         final var mxy = ma.getElementAt(0, 1);
4466         final var mxz = ma.getElementAt(0, 2);
4467         final var myx = ma.getElementAt(1, 0);
4468         final var myz = ma.getElementAt(1, 2);
4469         final var mzx = ma.getElementAt(2, 0);
4470         final var mzy = ma.getElementAt(2, 1);
4471 
4472         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
4473                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4474 
4475         // check default values
4476         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4477         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4478         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4479         final var bx1 = calibrator.getBiasXAsAcceleration();
4480         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4481         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4482         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4483         calibrator.getBiasXAsAcceleration(bx2);
4484         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4485         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4486         final var by1 = calibrator.getBiasYAsAcceleration();
4487         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4488         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4489         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4490         calibrator.getBiasYAsAcceleration(by2);
4491         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4492         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4493         final var bz1 = calibrator.getBiasZAsAcceleration();
4494         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4495         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4496         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4497         calibrator.getBiasZAsAcceleration(bz2);
4498         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4499         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4500         final var biasTriad1 = calibrator.getBiasAsTriad();
4501         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4502         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4503         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4504         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4505         final var biasTriad2 = new AccelerationTriad();
4506         calibrator.getBiasAsTriad(biasTriad2);
4507         assertEquals(biasTriad1, biasTriad2);
4508         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4509         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4510         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4511         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4512         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4513         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4514         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4515         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4516         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4517         final var bias1 = calibrator.getBias();
4518         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4519         final var bias2 = new double[3];
4520         calibrator.getBias(bias2);
4521         assertArrayEquals(bias1, bias2, 0.0);
4522         final var b1 = calibrator.getBiasAsMatrix();
4523         assertEquals(b1, ba);
4524         final var b2 = new Matrix(3, 1);
4525         calibrator.getBiasAsMatrix(b2);
4526         assertEquals(b1, b2);
4527         final var ma1 = new Matrix(3, 3);
4528         ma1.setSubmatrix(0, 0, 2, 2,
4529                 new double[]{sx, myx, mzx,
4530                         mxy, sy, mzy,
4531                         mxz, myz, sz});
4532         assertEquals(ma1, calibrator.getInitialMa());
4533         final var ma2 = new Matrix(3, 3);
4534         calibrator.getInitialMa(ma2);
4535         assertEquals(ma1, ma2);
4536         assertSame(measurements, calibrator.getMeasurements());
4537         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4538                 calibrator.getMeasurementType());
4539         assertFalse(calibrator.isOrderedMeasurementsRequired());
4540         assertFalse(calibrator.isQualityScoresRequired());
4541         assertTrue(calibrator.isCommonAxisUsed());
4542         assertNull(calibrator.getListener());
4543         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4544         assertFalse(calibrator.isReady());
4545         assertFalse(calibrator.isRunning());
4546         assertNull(calibrator.getEstimatedMa());
4547         assertNull(calibrator.getEstimatedSx());
4548         assertNull(calibrator.getEstimatedSy());
4549         assertNull(calibrator.getEstimatedSz());
4550         assertNull(calibrator.getEstimatedMxy());
4551         assertNull(calibrator.getEstimatedMxz());
4552         assertNull(calibrator.getEstimatedMyx());
4553         assertNull(calibrator.getEstimatedMyz());
4554         assertNull(calibrator.getEstimatedMzx());
4555         assertNull(calibrator.getEstimatedMzy());
4556         assertNull(calibrator.getEstimatedCovariance());
4557         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4558         assertNull(calibrator.getGroundTruthGravityNorm());
4559         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4560         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4561         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4562     }
4563 
4564     @Test
4565     void testConstructor46() throws WrongSizeException {
4566         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4567 
4568         final var ba = generateBa();
4569         final var biasX = ba.getElementAtIndex(0);
4570         final var biasY = ba.getElementAtIndex(1);
4571         final var biasZ = ba.getElementAtIndex(2);
4572 
4573         final var ma = generateMaCommonAxis();
4574         final var sx = ma.getElementAt(0, 0);
4575         final var sy = ma.getElementAt(1, 1);
4576         final var sz = ma.getElementAt(2, 2);
4577         final var mxy = ma.getElementAt(0, 1);
4578         final var mxz = ma.getElementAt(0, 2);
4579         final var myx = ma.getElementAt(1, 0);
4580         final var myz = ma.getElementAt(1, 2);
4581         final var mzx = ma.getElementAt(2, 0);
4582         final var mzy = ma.getElementAt(2, 1);
4583 
4584         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
4585                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
4586 
4587         // check default values
4588         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4589         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4590         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4591         final var bx1 = calibrator.getBiasXAsAcceleration();
4592         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4593         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4594         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4595         calibrator.getBiasXAsAcceleration(bx2);
4596         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4597         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4598         final var by1 = calibrator.getBiasYAsAcceleration();
4599         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4600         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4601         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4602         calibrator.getBiasYAsAcceleration(by2);
4603         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4604         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4605         final var bz1 = calibrator.getBiasZAsAcceleration();
4606         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4607         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4608         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4609         calibrator.getBiasZAsAcceleration(bz2);
4610         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4611         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4612         final var biasTriad1 = calibrator.getBiasAsTriad();
4613         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4614         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4615         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4616         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4617         final var biasTriad2 = new AccelerationTriad();
4618         calibrator.getBiasAsTriad(biasTriad2);
4619         assertEquals(biasTriad1, biasTriad2);
4620         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4621         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4622         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4623         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4624         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4625         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4626         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4627         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4628         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4629         final var bias1 = calibrator.getBias();
4630         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4631         final var bias2 = new double[3];
4632         calibrator.getBias(bias2);
4633         assertArrayEquals(bias1, bias2, 0.0);
4634         final var b1 = calibrator.getBiasAsMatrix();
4635         assertEquals(b1, ba);
4636         final var b2 = new Matrix(3, 1);
4637         calibrator.getBiasAsMatrix(b2);
4638         assertEquals(b1, b2);
4639         final var ma1 = new Matrix(3, 3);
4640         ma1.setSubmatrix(0, 0, 2, 2,
4641                 new double[]{sx, myx, mzx,
4642                         mxy, sy, mzy,
4643                         mxz, myz, sz});
4644         assertEquals(ma1, calibrator.getInitialMa());
4645         final var ma2 = new Matrix(3, 3);
4646         calibrator.getInitialMa(ma2);
4647         assertEquals(ma1, ma2);
4648         assertSame(measurements, calibrator.getMeasurements());
4649         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4650                 calibrator.getMeasurementType());
4651         assertFalse(calibrator.isOrderedMeasurementsRequired());
4652         assertFalse(calibrator.isQualityScoresRequired());
4653         assertTrue(calibrator.isCommonAxisUsed());
4654         assertSame(this, calibrator.getListener());
4655         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
4656         assertFalse(calibrator.isReady());
4657         assertFalse(calibrator.isRunning());
4658         assertNull(calibrator.getEstimatedMa());
4659         assertNull(calibrator.getEstimatedSx());
4660         assertNull(calibrator.getEstimatedSy());
4661         assertNull(calibrator.getEstimatedSz());
4662         assertNull(calibrator.getEstimatedMxy());
4663         assertNull(calibrator.getEstimatedMxz());
4664         assertNull(calibrator.getEstimatedMyx());
4665         assertNull(calibrator.getEstimatedMyz());
4666         assertNull(calibrator.getEstimatedMzx());
4667         assertNull(calibrator.getEstimatedMzy());
4668         assertNull(calibrator.getEstimatedCovariance());
4669         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4670         assertNull(calibrator.getGroundTruthGravityNorm());
4671         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4672         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4673         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4674     }
4675 
4676     @Test
4677     void testConstructor47() throws WrongSizeException {
4678         final var ba = generateBa();
4679         final var biasX = ba.getElementAtIndex(0);
4680         final var biasY = ba.getElementAtIndex(1);
4681         final var biasZ = ba.getElementAtIndex(2);
4682 
4683         final var ma = generateMaCommonAxis();
4684         final var sx = ma.getElementAt(0, 0);
4685         final var sy = ma.getElementAt(1, 1);
4686         final var sz = ma.getElementAt(2, 2);
4687         final var mxy = ma.getElementAt(0, 1);
4688         final var mxz = ma.getElementAt(0, 2);
4689         final var myx = ma.getElementAt(1, 0);
4690         final var myz = ma.getElementAt(1, 2);
4691         final var mzx = ma.getElementAt(2, 0);
4692         final var mzy = ma.getElementAt(2, 1);
4693 
4694         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4695         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4696         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4697 
4698         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz,
4699                 mxy, mxz, myx, myz, mzx, mzy);
4700 
4701         // check default values
4702         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4703         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4704         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4705         final var bx1 = calibrator.getBiasXAsAcceleration();
4706         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4707         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4708         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4709         calibrator.getBiasXAsAcceleration(bx2);
4710         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4711         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4712         final var by1 = calibrator.getBiasYAsAcceleration();
4713         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4714         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4715         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4716         calibrator.getBiasYAsAcceleration(by2);
4717         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4718         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4719         final var bz1 = calibrator.getBiasZAsAcceleration();
4720         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4721         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4722         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4723         calibrator.getBiasZAsAcceleration(bz2);
4724         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4725         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4726         final var biasTriad1 = calibrator.getBiasAsTriad();
4727         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4728         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4729         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4730         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4731         final var biasTriad2 = new AccelerationTriad();
4732         calibrator.getBiasAsTriad(biasTriad2);
4733         assertEquals(biasTriad1, biasTriad2);
4734         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4735         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4736         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4737         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4738         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4739         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4740         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4741         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4742         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4743         final var bias1 = calibrator.getBias();
4744         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4745         final var bias2 = new double[3];
4746         calibrator.getBias(bias2);
4747         assertArrayEquals(bias1, bias2, 0.0);
4748         final var b1 = calibrator.getBiasAsMatrix();
4749         assertEquals(b1, ba);
4750         final var b2 = new Matrix(3, 1);
4751         calibrator.getBiasAsMatrix(b2);
4752         assertEquals(b1, b2);
4753         final var ma1 = new Matrix(3, 3);
4754         ma1.setSubmatrix(0, 0, 2, 2,
4755                 new double[]{sx, myx, mzx,
4756                         mxy, sy, mzy,
4757                         mxz, myz, sz});
4758         assertEquals(ma1, calibrator.getInitialMa());
4759         final var ma2 = new Matrix(3, 3);
4760         calibrator.getInitialMa(ma2);
4761         assertEquals(ma1, ma2);
4762         assertNull(calibrator.getMeasurements());
4763         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4764                 calibrator.getMeasurementType());
4765         assertFalse(calibrator.isOrderedMeasurementsRequired());
4766         assertFalse(calibrator.isQualityScoresRequired());
4767         assertFalse(calibrator.isCommonAxisUsed());
4768         assertNull(calibrator.getListener());
4769         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4770         assertFalse(calibrator.isReady());
4771         assertFalse(calibrator.isRunning());
4772         assertNull(calibrator.getEstimatedMa());
4773         assertNull(calibrator.getEstimatedSx());
4774         assertNull(calibrator.getEstimatedSy());
4775         assertNull(calibrator.getEstimatedSz());
4776         assertNull(calibrator.getEstimatedMxy());
4777         assertNull(calibrator.getEstimatedMxz());
4778         assertNull(calibrator.getEstimatedMyx());
4779         assertNull(calibrator.getEstimatedMyz());
4780         assertNull(calibrator.getEstimatedMzx());
4781         assertNull(calibrator.getEstimatedMzy());
4782         assertNull(calibrator.getEstimatedCovariance());
4783         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4784         assertNull(calibrator.getGroundTruthGravityNorm());
4785         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4786         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4787         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4788     }
4789 
4790     @Test
4791     void testConstructor48() throws WrongSizeException {
4792         final var ba = generateBa();
4793         final var biasX = ba.getElementAtIndex(0);
4794         final var biasY = ba.getElementAtIndex(1);
4795         final var biasZ = ba.getElementAtIndex(2);
4796 
4797         final var ma = generateMaCommonAxis();
4798         final var sx = ma.getElementAt(0, 0);
4799         final var sy = ma.getElementAt(1, 1);
4800         final var sz = ma.getElementAt(2, 2);
4801         final var mxy = ma.getElementAt(0, 1);
4802         final var mxz = ma.getElementAt(0, 2);
4803         final var myx = ma.getElementAt(1, 0);
4804         final var myz = ma.getElementAt(1, 2);
4805         final var mzx = ma.getElementAt(2, 0);
4806         final var mzy = ma.getElementAt(2, 1);
4807 
4808         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4809         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4810         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4811 
4812         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bx, by, bz, sx, sy, sz,
4813                 mxy, mxz, myx, myz, mzx, mzy, this);
4814 
4815         // check default values
4816         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4817         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4818         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4819         final var bx1 = calibrator.getBiasXAsAcceleration();
4820         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4821         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4822         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4823         calibrator.getBiasXAsAcceleration(bx2);
4824         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4825         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4826         final var by1 = calibrator.getBiasYAsAcceleration();
4827         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4828         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4829         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4830         calibrator.getBiasYAsAcceleration(by2);
4831         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4832         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4833         final var bz1 = calibrator.getBiasZAsAcceleration();
4834         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4835         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4836         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4837         calibrator.getBiasZAsAcceleration(bz2);
4838         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4839         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4840         final var biasTriad1 = calibrator.getBiasAsTriad();
4841         assertEquals(biasTriad1.getValueX(), biasX, 0.0);
4842         assertEquals(biasTriad1.getValueY(), biasY, 0.0);
4843         assertEquals(biasTriad1.getValueZ(), biasZ, 0.0);
4844         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4845         final var biasTriad2 = new AccelerationTriad();
4846         calibrator.getBiasAsTriad(biasTriad2);
4847         assertEquals(biasTriad1, biasTriad2);
4848         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4849         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4850         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4851         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4852         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4853         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4854         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4855         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4856         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4857         final var bias1 = calibrator.getBias();
4858         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4859         final var bias2 = new double[3];
4860         calibrator.getBias(bias2);
4861         assertArrayEquals(bias1, bias2, 0.0);
4862         final var b1 = calibrator.getBiasAsMatrix();
4863         assertEquals(b1, ba);
4864         final var b2 = new Matrix(3, 1);
4865         calibrator.getBiasAsMatrix(b2);
4866         assertEquals(b1, b2);
4867         final var ma1 = new Matrix(3, 3);
4868         ma1.setSubmatrix(0, 0, 2, 2,
4869                 new double[]{sx, myx, mzx,
4870                         mxy, sy, mzy,
4871                         mxz, myz, sz});
4872         assertEquals(calibrator.getInitialMa(), ma1);
4873         final var ma2 = new Matrix(3, 3);
4874         calibrator.getInitialMa(ma2);
4875         assertEquals(ma1, ma2);
4876         assertNull(calibrator.getMeasurements());
4877         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4878                 calibrator.getMeasurementType());
4879         assertFalse(calibrator.isOrderedMeasurementsRequired());
4880         assertFalse(calibrator.isQualityScoresRequired());
4881         assertFalse(calibrator.isCommonAxisUsed());
4882         assertSame(this, calibrator.getListener());
4883         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
4884         assertFalse(calibrator.isReady());
4885         assertFalse(calibrator.isRunning());
4886         assertNull(calibrator.getEstimatedMa());
4887         assertNull(calibrator.getEstimatedSx());
4888         assertNull(calibrator.getEstimatedSy());
4889         assertNull(calibrator.getEstimatedSz());
4890         assertNull(calibrator.getEstimatedMxy());
4891         assertNull(calibrator.getEstimatedMxz());
4892         assertNull(calibrator.getEstimatedMyx());
4893         assertNull(calibrator.getEstimatedMyz());
4894         assertNull(calibrator.getEstimatedMzx());
4895         assertNull(calibrator.getEstimatedMzy());
4896         assertNull(calibrator.getEstimatedCovariance());
4897         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
4898         assertNull(calibrator.getGroundTruthGravityNorm());
4899         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4900         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4901         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
4902     }
4903 
4904     @Test
4905     void testConstructor49() throws WrongSizeException {
4906 
4907         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
4908 
4909         final var ba = generateBa();
4910         final var biasX = ba.getElementAtIndex(0);
4911         final var biasY = ba.getElementAtIndex(1);
4912         final var biasZ = ba.getElementAtIndex(2);
4913 
4914         final var ma = generateMaCommonAxis();
4915         final var sx = ma.getElementAt(0, 0);
4916         final var sy = ma.getElementAt(1, 1);
4917         final var sz = ma.getElementAt(2, 2);
4918         final var mxy = ma.getElementAt(0, 1);
4919         final var mxz = ma.getElementAt(0, 2);
4920         final var myx = ma.getElementAt(1, 0);
4921         final var myz = ma.getElementAt(1, 2);
4922         final var mzx = ma.getElementAt(2, 0);
4923         final var mzy = ma.getElementAt(2, 1);
4924 
4925         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4926         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4927         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
4928 
4929         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz,
4930                 mxy, mxz, myx, myz, mzx, mzy);
4931 
4932         // check default values
4933         assertEquals(biasX, calibrator.getBiasX(), 0.0);
4934         assertEquals(biasY, calibrator.getBiasY(), 0.0);
4935         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
4936         final var bx1 = calibrator.getBiasXAsAcceleration();
4937         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
4938         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
4939         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4940         calibrator.getBiasXAsAcceleration(bx2);
4941         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
4942         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
4943         final var by1 = calibrator.getBiasYAsAcceleration();
4944         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
4945         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
4946         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4947         calibrator.getBiasYAsAcceleration(by2);
4948         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
4949         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
4950         final var bz1 = calibrator.getBiasZAsAcceleration();
4951         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
4952         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
4953         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
4954         calibrator.getBiasZAsAcceleration(bz2);
4955         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
4956         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
4957         final var biasTriad1 = calibrator.getBiasAsTriad();
4958         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
4959         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
4960         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
4961         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
4962         final var biasTriad2 = new AccelerationTriad();
4963         calibrator.getBiasAsTriad(biasTriad2);
4964         assertEquals(biasTriad1, biasTriad2);
4965         assertEquals(sx, calibrator.getInitialSx(), 0.0);
4966         assertEquals(sy, calibrator.getInitialSy(), 0.0);
4967         assertEquals(sz, calibrator.getInitialSz(), 0.0);
4968         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
4969         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
4970         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
4971         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
4972         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
4973         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
4974         final var bias1 = calibrator.getBias();
4975         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4976         final var bias2 = new double[3];
4977         calibrator.getBias(bias2);
4978         assertArrayEquals(bias1, bias2, 0.0);
4979         final var b1 = calibrator.getBiasAsMatrix();
4980         assertEquals(b1, ba);
4981         final var b2 = new Matrix(3, 1);
4982         calibrator.getBiasAsMatrix(b2);
4983         assertEquals(b1, b2);
4984         final var ma1 = new Matrix(3, 3);
4985         ma1.setSubmatrix(0, 0, 2, 2,
4986                 new double[]{sx, myx, mzx,
4987                         mxy, sy, mzy,
4988                         mxz, myz, sz});
4989         assertEquals(ma1, calibrator.getInitialMa());
4990         final var ma2 = new Matrix(3, 3);
4991         calibrator.getInitialMa(ma2);
4992         assertEquals(ma1, ma2);
4993         assertSame(measurements, calibrator.getMeasurements());
4994         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
4995                 calibrator.getMeasurementType());
4996         assertFalse(calibrator.isOrderedMeasurementsRequired());
4997         assertFalse(calibrator.isQualityScoresRequired());
4998         assertFalse(calibrator.isCommonAxisUsed());
4999         assertNull(calibrator.getListener());
5000         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5001         assertFalse(calibrator.isReady());
5002         assertFalse(calibrator.isRunning());
5003         assertNull(calibrator.getEstimatedMa());
5004         assertNull(calibrator.getEstimatedSx());
5005         assertNull(calibrator.getEstimatedSy());
5006         assertNull(calibrator.getEstimatedSz());
5007         assertNull(calibrator.getEstimatedMxy());
5008         assertNull(calibrator.getEstimatedMxz());
5009         assertNull(calibrator.getEstimatedMyx());
5010         assertNull(calibrator.getEstimatedMyz());
5011         assertNull(calibrator.getEstimatedMzx());
5012         assertNull(calibrator.getEstimatedMzy());
5013         assertNull(calibrator.getEstimatedCovariance());
5014         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5015         assertNull(calibrator.getGroundTruthGravityNorm());
5016         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5017         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5018         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5019     }
5020 
5021     @Test
5022     void testConstructor50() throws WrongSizeException {
5023 
5024         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5025 
5026         final var ba = generateBa();
5027         final var biasX = ba.getElementAtIndex(0);
5028         final var biasY = ba.getElementAtIndex(1);
5029         final var biasZ = ba.getElementAtIndex(2);
5030 
5031         final var ma = generateMaCommonAxis();
5032         final var sx = ma.getElementAt(0, 0);
5033         final var sy = ma.getElementAt(1, 1);
5034         final var sz = ma.getElementAt(2, 2);
5035         final var mxy = ma.getElementAt(0, 1);
5036         final var mxz = ma.getElementAt(0, 2);
5037         final var myx = ma.getElementAt(1, 0);
5038         final var myz = ma.getElementAt(1, 2);
5039         final var mzx = ma.getElementAt(2, 0);
5040         final var mzy = ma.getElementAt(2, 1);
5041 
5042         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5043         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5044         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5045 
5046         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bx, by, bz, sx, sy, sz,
5047                 mxy, mxz, myx, myz, mzx, mzy, this);
5048 
5049         // check default values
5050         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5051         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5052         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5053         final var bx1 = calibrator.getBiasXAsAcceleration();
5054         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5055         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5056         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5057         calibrator.getBiasXAsAcceleration(bx2);
5058         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5059         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5060         final var by1 = calibrator.getBiasYAsAcceleration();
5061         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5062         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5063         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5064         calibrator.getBiasYAsAcceleration(by2);
5065         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5066         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5067         final var bz1 = calibrator.getBiasZAsAcceleration();
5068         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5069         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5070         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5071         calibrator.getBiasZAsAcceleration(bz2);
5072         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5073         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5074         final var biasTriad1 = calibrator.getBiasAsTriad();
5075         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5076         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5077         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5078         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5079         final var biasTriad2 = new AccelerationTriad();
5080         calibrator.getBiasAsTriad(biasTriad2);
5081         assertEquals(biasTriad1, biasTriad2);
5082         assertEquals(sx, calibrator.getInitialSx(), 0.0);
5083         assertEquals(sy, calibrator.getInitialSy(), 0.0);
5084         assertEquals(sz, calibrator.getInitialSz(), 0.0);
5085         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5086         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5087         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5088         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5089         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5090         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5091         final var bias1 = calibrator.getBias();
5092         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5093         final var bias2 = new double[3];
5094         calibrator.getBias(bias2);
5095         assertArrayEquals(bias1, bias2, 0.0);
5096         final var b1 = calibrator.getBiasAsMatrix();
5097         assertEquals(b1, ba);
5098         final var b2 = new Matrix(3, 1);
5099         calibrator.getBiasAsMatrix(b2);
5100         assertEquals(b1, b2);
5101         final var ma1 = new Matrix(3, 3);
5102         ma1.setSubmatrix(0, 0, 2, 2,
5103                 new double[]{sx, myx, mzx,
5104                         mxy, sy, mzy,
5105                         mxz, myz, sz});
5106         assertEquals(ma1, calibrator.getInitialMa());
5107         final var ma2 = new Matrix(3, 3);
5108         calibrator.getInitialMa(ma2);
5109         assertEquals(ma1, ma2);
5110         assertSame(measurements, calibrator.getMeasurements());
5111         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5112                 calibrator.getMeasurementType());
5113         assertFalse(calibrator.isOrderedMeasurementsRequired());
5114         assertFalse(calibrator.isQualityScoresRequired());
5115         assertFalse(calibrator.isCommonAxisUsed());
5116         assertSame(this, calibrator.getListener());
5117         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5118         assertFalse(calibrator.isReady());
5119         assertFalse(calibrator.isRunning());
5120         assertNull(calibrator.getEstimatedMa());
5121         assertNull(calibrator.getEstimatedSx());
5122         assertNull(calibrator.getEstimatedSy());
5123         assertNull(calibrator.getEstimatedSz());
5124         assertNull(calibrator.getEstimatedMxy());
5125         assertNull(calibrator.getEstimatedMxz());
5126         assertNull(calibrator.getEstimatedMyx());
5127         assertNull(calibrator.getEstimatedMyz());
5128         assertNull(calibrator.getEstimatedMzx());
5129         assertNull(calibrator.getEstimatedMzy());
5130         assertNull(calibrator.getEstimatedCovariance());
5131         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5132         assertNull(calibrator.getGroundTruthGravityNorm());
5133         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5134         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5135         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5136     }
5137 
5138     @Test
5139     void testConstructor51() throws WrongSizeException {
5140         final var ba = generateBa();
5141         final var biasX = ba.getElementAtIndex(0);
5142         final var biasY = ba.getElementAtIndex(1);
5143         final var biasZ = ba.getElementAtIndex(2);
5144 
5145         final var ma = generateMaCommonAxis();
5146         final var sx = ma.getElementAt(0, 0);
5147         final var sy = ma.getElementAt(1, 1);
5148         final var sz = ma.getElementAt(2, 2);
5149         final var mxy = ma.getElementAt(0, 1);
5150         final var mxz = ma.getElementAt(0, 2);
5151         final var myx = ma.getElementAt(1, 0);
5152         final var myz = ma.getElementAt(1, 2);
5153         final var mzx = ma.getElementAt(2, 0);
5154         final var mzy = ma.getElementAt(2, 1);
5155 
5156         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5157         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5158         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5159 
5160         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
5161                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5162 
5163         // check default values
5164         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5165         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5166         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5167         final var bx1 = calibrator.getBiasXAsAcceleration();
5168         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5169         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5170         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5171         calibrator.getBiasXAsAcceleration(bx2);
5172         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5173         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5174         final var by1 = calibrator.getBiasYAsAcceleration();
5175         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5176         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5177         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5178         calibrator.getBiasYAsAcceleration(by2);
5179         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5180         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5181         final var bz1 = calibrator.getBiasZAsAcceleration();
5182         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5183         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5184         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5185         calibrator.getBiasZAsAcceleration(bz2);
5186         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5187         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5188         final var biasTriad1 = calibrator.getBiasAsTriad();
5189         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5190         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5191         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5192         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5193         final var biasTriad2 = new AccelerationTriad();
5194         calibrator.getBiasAsTriad(biasTriad2);
5195         assertEquals(biasTriad1, biasTriad2);
5196         assertEquals(sx, calibrator.getInitialSx(), 0.0);
5197         assertEquals(sy, calibrator.getInitialSy(), 0.0);
5198         assertEquals(sz, calibrator.getInitialSz(), 0.0);
5199         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5200         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5201         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5202         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5203         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5204         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5205         final var bias1 = calibrator.getBias();
5206         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5207         final var bias2 = new double[3];
5208         calibrator.getBias(bias2);
5209         assertArrayEquals(bias1, bias2, 0.0);
5210         final var b1 = calibrator.getBiasAsMatrix();
5211         assertEquals(b1, ba);
5212         final var b2 = new Matrix(3, 1);
5213         calibrator.getBiasAsMatrix(b2);
5214         assertEquals(b1, b2);
5215         final var ma1 = new Matrix(3, 3);
5216         ma1.setSubmatrix(0, 0, 2, 2,
5217                 new double[]{sx, myx, mzx,
5218                         mxy, sy, mzy,
5219                         mxz, myz, sz});
5220         assertEquals(ma1, calibrator.getInitialMa());
5221         final var ma2 = new Matrix(3, 3);
5222         calibrator.getInitialMa(ma2);
5223         assertEquals(ma1, ma2);
5224         assertNull(calibrator.getMeasurements());
5225         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5226                 calibrator.getMeasurementType());
5227         assertFalse(calibrator.isOrderedMeasurementsRequired());
5228         assertFalse(calibrator.isQualityScoresRequired());
5229         assertTrue(calibrator.isCommonAxisUsed());
5230         assertNull(calibrator.getListener());
5231         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5232         assertFalse(calibrator.isReady());
5233         assertFalse(calibrator.isRunning());
5234         assertNull(calibrator.getEstimatedMa());
5235         assertNull(calibrator.getEstimatedSx());
5236         assertNull(calibrator.getEstimatedSy());
5237         assertNull(calibrator.getEstimatedSz());
5238         assertNull(calibrator.getEstimatedMxy());
5239         assertNull(calibrator.getEstimatedMxz());
5240         assertNull(calibrator.getEstimatedMyx());
5241         assertNull(calibrator.getEstimatedMyz());
5242         assertNull(calibrator.getEstimatedMzx());
5243         assertNull(calibrator.getEstimatedMzy());
5244         assertNull(calibrator.getEstimatedCovariance());
5245         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5246         assertNull(calibrator.getGroundTruthGravityNorm());
5247         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5248         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5249         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5250     }
5251 
5252     @Test
5253     void testConstructor52() throws WrongSizeException {
5254         final var ba = generateBa();
5255         final var biasX = ba.getElementAtIndex(0);
5256         final var biasY = ba.getElementAtIndex(1);
5257         final var biasZ = ba.getElementAtIndex(2);
5258 
5259         final var ma = generateMaCommonAxis();
5260         final var sx = ma.getElementAt(0, 0);
5261         final var sy = ma.getElementAt(1, 1);
5262         final var sz = ma.getElementAt(2, 2);
5263         final var mxy = ma.getElementAt(0, 1);
5264         final var mxz = ma.getElementAt(0, 2);
5265         final var myx = ma.getElementAt(1, 0);
5266         final var myz = ma.getElementAt(1, 2);
5267         final var mzx = ma.getElementAt(2, 0);
5268         final var mzy = ma.getElementAt(2, 1);
5269 
5270         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5271         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5272         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5273 
5274         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bx, by, bz,
5275                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5276 
5277         // check default values
5278         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5279         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5280         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5281         final var bx1 = calibrator.getBiasXAsAcceleration();
5282         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5283         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5284         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5285         calibrator.getBiasXAsAcceleration(bx2);
5286         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5287         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5288         final var by1 = calibrator.getBiasYAsAcceleration();
5289         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5290         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5291         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5292         calibrator.getBiasYAsAcceleration(by2);
5293         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5294         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5295         final var bz1 = calibrator.getBiasZAsAcceleration();
5296         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5297         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5298         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5299         calibrator.getBiasZAsAcceleration(bz2);
5300         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5301         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5302         final var biasTriad1 = calibrator.getBiasAsTriad();
5303         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5304         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5305         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5306         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5307         final var biasTriad2 = new AccelerationTriad();
5308         calibrator.getBiasAsTriad(biasTriad2);
5309         assertEquals(biasTriad1, biasTriad2);
5310         assertEquals(sx, calibrator.getInitialSx(), 0.0);
5311         assertEquals(sy, calibrator.getInitialSy(), 0.0);
5312         assertEquals(sz, calibrator.getInitialSz(), 0.0);
5313         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5314         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5315         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5316         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5317         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5318         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5319         final var bias1 = calibrator.getBias();
5320         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5321         final var bias2 = new double[3];
5322         calibrator.getBias(bias2);
5323         assertArrayEquals(bias1, bias2, 0.0);
5324         final var b1 = calibrator.getBiasAsMatrix();
5325         assertEquals(b1, ba);
5326         final var b2 = new Matrix(3, 1);
5327         calibrator.getBiasAsMatrix(b2);
5328         assertEquals(b1, b2);
5329         final var ma1 = new Matrix(3, 3);
5330         ma1.setSubmatrix(0, 0, 2, 2,
5331                 new double[]{sx, myx, mzx,
5332                         mxy, sy, mzy,
5333                         mxz, myz, sz});
5334         assertEquals(ma1, calibrator.getInitialMa());
5335         final var ma2 = new Matrix(3, 3);
5336         calibrator.getInitialMa(ma2);
5337         assertEquals(ma1, ma2);
5338         assertNull(calibrator.getMeasurements());
5339         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5340                 calibrator.getMeasurementType());
5341         assertFalse(calibrator.isOrderedMeasurementsRequired());
5342         assertFalse(calibrator.isQualityScoresRequired());
5343         assertTrue(calibrator.isCommonAxisUsed());
5344         assertSame(this, calibrator.getListener());
5345         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5346         assertFalse(calibrator.isReady());
5347         assertFalse(calibrator.isRunning());
5348         assertNull(calibrator.getEstimatedMa());
5349         assertNull(calibrator.getEstimatedSx());
5350         assertNull(calibrator.getEstimatedSy());
5351         assertNull(calibrator.getEstimatedSz());
5352         assertNull(calibrator.getEstimatedMxy());
5353         assertNull(calibrator.getEstimatedMxz());
5354         assertNull(calibrator.getEstimatedMyx());
5355         assertNull(calibrator.getEstimatedMyz());
5356         assertNull(calibrator.getEstimatedMzx());
5357         assertNull(calibrator.getEstimatedMzy());
5358         assertNull(calibrator.getEstimatedCovariance());
5359         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5360         assertNull(calibrator.getGroundTruthGravityNorm());
5361         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5362         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5363         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5364     }
5365 
5366     @Test
5367     void testConstructor53() throws WrongSizeException {
5368         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5369 
5370         final var ba = generateBa();
5371         final var biasX = ba.getElementAtIndex(0);
5372         final var biasY = ba.getElementAtIndex(1);
5373         final var biasZ = ba.getElementAtIndex(2);
5374 
5375         final var ma = generateMaCommonAxis();
5376         final var sx = ma.getElementAt(0, 0);
5377         final var sy = ma.getElementAt(1, 1);
5378         final var sz = ma.getElementAt(2, 2);
5379         final var mxy = ma.getElementAt(0, 1);
5380         final var mxz = ma.getElementAt(0, 2);
5381         final var myx = ma.getElementAt(1, 0);
5382         final var myz = ma.getElementAt(1, 2);
5383         final var mzx = ma.getElementAt(2, 0);
5384         final var mzy = ma.getElementAt(2, 1);
5385 
5386         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5387         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5388         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5389 
5390         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
5391                 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5392 
5393         // check default values
5394         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5395         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5396         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5397         final var bx1 = calibrator.getBiasXAsAcceleration();
5398         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5399         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5400         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5401         calibrator.getBiasXAsAcceleration(bx2);
5402         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5403         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5404         final var by1 = calibrator.getBiasYAsAcceleration();
5405         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5406         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5407         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5408         calibrator.getBiasYAsAcceleration(by2);
5409         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5410         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5411         final var bz1 = calibrator.getBiasZAsAcceleration();
5412         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5413         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5414         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5415         calibrator.getBiasZAsAcceleration(bz2);
5416         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5417         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5418         final var biasTriad1 = calibrator.getBiasAsTriad();
5419         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5420         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5421         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5422         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5423         final var biasTriad2 = new AccelerationTriad();
5424         calibrator.getBiasAsTriad(biasTriad2);
5425         assertEquals(biasTriad1, biasTriad2);
5426         assertEquals(sx, calibrator.getInitialSx(), 0.0);
5427         assertEquals(sy, calibrator.getInitialSy(), 0.0);
5428         assertEquals(sz, calibrator.getInitialSz(), 0.0);
5429         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5430         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5431         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5432         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5433         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5434         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5435         final var bias1 = calibrator.getBias();
5436         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5437         final var bias2 = new double[3];
5438         calibrator.getBias(bias2);
5439         assertArrayEquals(bias1, bias2, 0.0);
5440         final var b1 = calibrator.getBiasAsMatrix();
5441         assertEquals(b1, ba);
5442         final var b2 = new Matrix(3, 1);
5443         calibrator.getBiasAsMatrix(b2);
5444         assertEquals(b1, b2);
5445         final var ma1 = new Matrix(3, 3);
5446         ma1.setSubmatrix(0, 0, 2, 2,
5447                 new double[]{sx, myx, mzx,
5448                         mxy, sy, mzy,
5449                         mxz, myz, sz});
5450         assertEquals(ma1, calibrator.getInitialMa());
5451         final var ma2 = new Matrix(3, 3);
5452         calibrator.getInitialMa(ma2);
5453         assertEquals(ma1, ma2);
5454         assertSame(measurements, calibrator.getMeasurements());
5455         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5456                 calibrator.getMeasurementType());
5457         assertFalse(calibrator.isOrderedMeasurementsRequired());
5458         assertFalse(calibrator.isQualityScoresRequired());
5459         assertTrue(calibrator.isCommonAxisUsed());
5460         assertNull(calibrator.getListener());
5461         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5462         assertFalse(calibrator.isReady());
5463         assertFalse(calibrator.isRunning());
5464         assertNull(calibrator.getEstimatedMa());
5465         assertNull(calibrator.getEstimatedSx());
5466         assertNull(calibrator.getEstimatedSy());
5467         assertNull(calibrator.getEstimatedSz());
5468         assertNull(calibrator.getEstimatedMxy());
5469         assertNull(calibrator.getEstimatedMxz());
5470         assertNull(calibrator.getEstimatedMyx());
5471         assertNull(calibrator.getEstimatedMyz());
5472         assertNull(calibrator.getEstimatedMzx());
5473         assertNull(calibrator.getEstimatedMzy());
5474         assertNull(calibrator.getEstimatedCovariance());
5475         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5476         assertNull(calibrator.getGroundTruthGravityNorm());
5477         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5478         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5479         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5480     }
5481 
5482     @Test
5483     void testConstructor54() throws WrongSizeException {
5484         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5485 
5486         final var ba = generateBa();
5487         final var biasX = ba.getElementAtIndex(0);
5488         final var biasY = ba.getElementAtIndex(1);
5489         final var biasZ = ba.getElementAtIndex(2);
5490 
5491         final var ma = generateMaCommonAxis();
5492         final var sx = ma.getElementAt(0, 0);
5493         final var sy = ma.getElementAt(1, 1);
5494         final var sz = ma.getElementAt(2, 2);
5495         final var mxy = ma.getElementAt(0, 1);
5496         final var mxz = ma.getElementAt(0, 2);
5497         final var myx = ma.getElementAt(1, 0);
5498         final var myz = ma.getElementAt(1, 2);
5499         final var mzx = ma.getElementAt(2, 0);
5500         final var mzy = ma.getElementAt(2, 1);
5501 
5502         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5503         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5504         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
5505 
5506         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
5507                 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5508 
5509         // check default values
5510         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5511         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5512         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5513         final var bx1 = calibrator.getBiasXAsAcceleration();
5514         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5515         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5516         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5517         calibrator.getBiasXAsAcceleration(bx2);
5518         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5519         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5520         final var by1 = calibrator.getBiasYAsAcceleration();
5521         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5522         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5523         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5524         calibrator.getBiasYAsAcceleration(by2);
5525         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5526         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5527         final var bz1 = calibrator.getBiasZAsAcceleration();
5528         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5529         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5530         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5531         calibrator.getBiasZAsAcceleration(bz2);
5532         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5533         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5534         final var biasTriad1 = calibrator.getBiasAsTriad();
5535         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5536         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5537         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5538         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5539         final var biasTriad2 = new AccelerationTriad();
5540         calibrator.getBiasAsTriad(biasTriad2);
5541         assertEquals(biasTriad1, biasTriad2);
5542         assertEquals(sx, calibrator.getInitialSx(), 0.0);
5543         assertEquals(sy, calibrator.getInitialSy(), 0.0);
5544         assertEquals(sz, calibrator.getInitialSz(), 0.0);
5545         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
5546         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
5547         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
5548         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
5549         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
5550         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
5551         final var bias1 = calibrator.getBias();
5552         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5553         final var bias2 = new double[3];
5554         calibrator.getBias(bias2);
5555         assertArrayEquals(bias1, bias2, 0.0);
5556         final var b1 = calibrator.getBiasAsMatrix();
5557         assertEquals(b1, ba);
5558         final var b2 = new Matrix(3, 1);
5559         calibrator.getBiasAsMatrix(b2);
5560         assertEquals(b1, b2);
5561         final var ma1 = new Matrix(3, 3);
5562         ma1.setSubmatrix(0, 0, 2, 2,
5563                 new double[]{sx, myx, mzx,
5564                         mxy, sy, mzy,
5565                         mxz, myz, sz});
5566         assertEquals(ma1, calibrator.getInitialMa());
5567         final var ma2 = new Matrix(3, 3);
5568         calibrator.getInitialMa(ma2);
5569         assertEquals(ma1, ma2);
5570         assertSame(measurements, calibrator.getMeasurements());
5571         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5572                 calibrator.getMeasurementType());
5573         assertFalse(calibrator.isOrderedMeasurementsRequired());
5574         assertFalse(calibrator.isQualityScoresRequired());
5575         assertTrue(calibrator.isCommonAxisUsed());
5576         assertSame(this, calibrator.getListener());
5577         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
5578         assertFalse(calibrator.isReady());
5579         assertFalse(calibrator.isRunning());
5580         assertNull(calibrator.getEstimatedMa());
5581         assertNull(calibrator.getEstimatedSx());
5582         assertNull(calibrator.getEstimatedSy());
5583         assertNull(calibrator.getEstimatedSz());
5584         assertNull(calibrator.getEstimatedMxy());
5585         assertNull(calibrator.getEstimatedMxz());
5586         assertNull(calibrator.getEstimatedMyx());
5587         assertNull(calibrator.getEstimatedMyz());
5588         assertNull(calibrator.getEstimatedMzx());
5589         assertNull(calibrator.getEstimatedMzy());
5590         assertNull(calibrator.getEstimatedCovariance());
5591         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5592         assertNull(calibrator.getGroundTruthGravityNorm());
5593         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5594         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5595         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5596     }
5597 
5598     @Test
5599     void testConstructor55() throws WrongSizeException {
5600         final var ba = generateBa();
5601         final var bias = ba.getBuffer();
5602         final var biasX = ba.getElementAtIndex(0);
5603         final var biasY = ba.getElementAtIndex(1);
5604         final var biasZ = ba.getElementAtIndex(2);
5605 
5606         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bias);
5607 
5608         // check default values
5609         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5610         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5611         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5612         final var bx1 = calibrator.getBiasXAsAcceleration();
5613         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5614         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5615         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5616         calibrator.getBiasXAsAcceleration(bx2);
5617         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5618         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5619         final var by1 = calibrator.getBiasYAsAcceleration();
5620         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5621         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5622         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5623         calibrator.getBiasYAsAcceleration(by2);
5624         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5625         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5626         final var bz1 = calibrator.getBiasZAsAcceleration();
5627         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5628         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5629         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5630         calibrator.getBiasZAsAcceleration(bz2);
5631         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5632         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5633         final var biasTriad1 = calibrator.getBiasAsTriad();
5634         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5635         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5636         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5637         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5638         final var biasTriad2 = new AccelerationTriad();
5639         calibrator.getBiasAsTriad(biasTriad2);
5640         assertEquals(biasTriad1, biasTriad2);
5641         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5642         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5643         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5644         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5645         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5646         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5647         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5648         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5649         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5650         final var bias1 = calibrator.getBias();
5651         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5652         final var bias2 = new double[3];
5653         calibrator.getBias(bias2);
5654         assertArrayEquals(bias1, bias2, 0.0);
5655         final var b1 = calibrator.getBiasAsMatrix();
5656         assertEquals(b1, ba);
5657         final var b2 = new Matrix(3, 1);
5658         calibrator.getBiasAsMatrix(b2);
5659         assertEquals(b1, b2);
5660         final var ma1 = calibrator.getInitialMa();
5661         assertEquals(new Matrix(3, 3), ma1);
5662         final var ma2 = new Matrix(3, 3);
5663         calibrator.getInitialMa(ma2);
5664         assertEquals(ma1, ma2);
5665         assertNull(calibrator.getMeasurements());
5666         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5667                 calibrator.getMeasurementType());
5668         assertFalse(calibrator.isOrderedMeasurementsRequired());
5669         assertFalse(calibrator.isQualityScoresRequired());
5670         assertFalse(calibrator.isCommonAxisUsed());
5671         assertNull(calibrator.getListener());
5672         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5673         assertFalse(calibrator.isReady());
5674         assertFalse(calibrator.isRunning());
5675         assertNull(calibrator.getEstimatedMa());
5676         assertNull(calibrator.getEstimatedSx());
5677         assertNull(calibrator.getEstimatedSy());
5678         assertNull(calibrator.getEstimatedSz());
5679         assertNull(calibrator.getEstimatedMxy());
5680         assertNull(calibrator.getEstimatedMxz());
5681         assertNull(calibrator.getEstimatedMyx());
5682         assertNull(calibrator.getEstimatedMyz());
5683         assertNull(calibrator.getEstimatedMzx());
5684         assertNull(calibrator.getEstimatedMzy());
5685         assertNull(calibrator.getEstimatedCovariance());
5686         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5687         assertNull(calibrator.getGroundTruthGravityNorm());
5688         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5689         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5690         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5691 
5692         // Force IllegalArgumentException
5693         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
5694                 new double[1]));
5695     }
5696 
5697     @Test
5698     void testConstructor56() throws WrongSizeException {
5699         final var ba = generateBa();
5700         final var bias = ba.getBuffer();
5701         final var biasX = ba.getElementAtIndex(0);
5702         final var biasY = ba.getElementAtIndex(1);
5703         final var biasZ = ba.getElementAtIndex(2);
5704 
5705         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(bias, this);
5706 
5707         // check default values
5708         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5709         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5710         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5711         final var bx1 = calibrator.getBiasXAsAcceleration();
5712         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5713         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5714         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5715         calibrator.getBiasXAsAcceleration(bx2);
5716         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5717         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5718         final var by1 = calibrator.getBiasYAsAcceleration();
5719         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5720         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5721         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5722         calibrator.getBiasYAsAcceleration(by2);
5723         assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5724         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5725         final var bz1 = calibrator.getBiasZAsAcceleration();
5726         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5727         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5728         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5729         calibrator.getBiasZAsAcceleration(bz2);
5730         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5731         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5732         final var biasTriad1 = calibrator.getBiasAsTriad();
5733         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5734         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5735         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5736         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5737         final var biasTriad2 = new AccelerationTriad();
5738         calibrator.getBiasAsTriad(biasTriad2);
5739         assertEquals(biasTriad1, biasTriad2);
5740         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5741         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5742         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5743         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5744         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5745         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5746         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5747         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5748         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5749         final var bias1 = calibrator.getBias();
5750         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5751         final var bias2 = new double[3];
5752         calibrator.getBias(bias2);
5753         assertArrayEquals(bias1, bias2, 0.0);
5754         final var b1 = calibrator.getBiasAsMatrix();
5755         assertEquals(b1, ba);
5756         final var b2 = new Matrix(3, 1);
5757         calibrator.getBiasAsMatrix(b2);
5758         assertEquals(b1, b2);
5759         final var ma1 = calibrator.getInitialMa();
5760         assertEquals(new Matrix(3, 3), ma1);
5761         final var ma2 = new Matrix(3, 3);
5762         calibrator.getInitialMa(ma2);
5763         assertEquals(ma1, ma2);
5764         assertNull(calibrator.getMeasurements());
5765         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5766                 calibrator.getMeasurementType());
5767         assertFalse(calibrator.isOrderedMeasurementsRequired());
5768         assertFalse(calibrator.isQualityScoresRequired());
5769         assertFalse(calibrator.isCommonAxisUsed());
5770         assertSame(this, calibrator.getListener());
5771         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5772         assertFalse(calibrator.isReady());
5773         assertFalse(calibrator.isRunning());
5774         assertNull(calibrator.getEstimatedMa());
5775         assertNull(calibrator.getEstimatedSx());
5776         assertNull(calibrator.getEstimatedSy());
5777         assertNull(calibrator.getEstimatedSz());
5778         assertNull(calibrator.getEstimatedMxy());
5779         assertNull(calibrator.getEstimatedMxz());
5780         assertNull(calibrator.getEstimatedMyx());
5781         assertNull(calibrator.getEstimatedMyz());
5782         assertNull(calibrator.getEstimatedMzx());
5783         assertNull(calibrator.getEstimatedMzy());
5784         assertNull(calibrator.getEstimatedCovariance());
5785         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5786         assertNull(calibrator.getGroundTruthGravityNorm());
5787         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5788         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5789         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5790 
5791         // Force IllegalArgumentException
5792         assertThrows(IllegalArgumentException.class,
5793                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(new double[1], this));
5794     }
5795 
5796     @Test
5797     void testConstructor57() throws WrongSizeException {
5798         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5799 
5800         final var ba = generateBa();
5801         final var bias = ba.getBuffer();
5802         final var biasX = ba.getElementAtIndex(0);
5803         final var biasY = ba.getElementAtIndex(1);
5804         final var biasZ = ba.getElementAtIndex(2);
5805 
5806         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bias);
5807 
5808         // check default values
5809         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5810         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5811         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5812         final var bx1 = calibrator.getBiasXAsAcceleration();
5813         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5814         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5815         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5816         calibrator.getBiasXAsAcceleration(bx2);
5817         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5818         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5819         final var by1 = calibrator.getBiasYAsAcceleration();
5820         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5821         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5822         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5823         calibrator.getBiasYAsAcceleration(by2);
5824         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5825         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5826         final var bz1 = calibrator.getBiasZAsAcceleration();
5827         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5828         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5829         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5830         calibrator.getBiasZAsAcceleration(bz2);
5831         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5832         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5833         final var biasTriad1 = calibrator.getBiasAsTriad();
5834         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5835         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5836         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5837         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5838         final var biasTriad2 = new AccelerationTriad();
5839         calibrator.getBiasAsTriad(biasTriad2);
5840         assertEquals(biasTriad1, biasTriad2);
5841         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5842         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5843         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5844         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5845         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5846         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5847         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5848         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5849         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5850         final var bias1 = calibrator.getBias();
5851         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5852         final var bias2 = new double[3];
5853         calibrator.getBias(bias2);
5854         assertArrayEquals(bias1, bias2, 0.0);
5855         final var b1 = calibrator.getBiasAsMatrix();
5856         assertEquals(b1, ba);
5857         final var b2 = new Matrix(3, 1);
5858         calibrator.getBiasAsMatrix(b2);
5859         assertEquals(b1, b2);
5860         final var ma1 = calibrator.getInitialMa();
5861         assertEquals(new Matrix(3, 3), ma1);
5862         final var ma2 = new Matrix(3, 3);
5863         calibrator.getInitialMa(ma2);
5864         assertEquals(ma1, ma2);
5865         assertSame(measurements, calibrator.getMeasurements());
5866         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5867                 calibrator.getMeasurementType());
5868         assertFalse(calibrator.isOrderedMeasurementsRequired());
5869         assertFalse(calibrator.isQualityScoresRequired());
5870         assertFalse(calibrator.isCommonAxisUsed());
5871         assertNull(calibrator.getListener());
5872         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5873         assertFalse(calibrator.isReady());
5874         assertFalse(calibrator.isRunning());
5875         assertNull(calibrator.getEstimatedMa());
5876         assertNull(calibrator.getEstimatedSx());
5877         assertNull(calibrator.getEstimatedSy());
5878         assertNull(calibrator.getEstimatedSz());
5879         assertNull(calibrator.getEstimatedMxy());
5880         assertNull(calibrator.getEstimatedMxz());
5881         assertNull(calibrator.getEstimatedMyx());
5882         assertNull(calibrator.getEstimatedMyz());
5883         assertNull(calibrator.getEstimatedMzx());
5884         assertNull(calibrator.getEstimatedMzy());
5885         assertNull(calibrator.getEstimatedCovariance());
5886         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5887         assertNull(calibrator.getGroundTruthGravityNorm());
5888         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5889         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5890         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5891 
5892         // Force IllegalArgumentException
5893         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
5894                 measurements, new double[1]));
5895     }
5896 
5897     @Test
5898     void testConstructor58() throws WrongSizeException {
5899         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
5900 
5901         final var ba = generateBa();
5902         final var bias = ba.getBuffer();
5903         final var biasX = ba.getElementAtIndex(0);
5904         final var biasY = ba.getElementAtIndex(1);
5905         final var biasZ = ba.getElementAtIndex(2);
5906 
5907         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, bias, this);
5908 
5909         // check default values
5910         assertEquals(biasX, calibrator.getBiasX(), 0.0);
5911         assertEquals(biasY, calibrator.getBiasY(), 0.0);
5912         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
5913         final var bx1 = calibrator.getBiasXAsAcceleration();
5914         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
5915         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
5916         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5917         calibrator.getBiasXAsAcceleration(bx2);
5918         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
5919         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
5920         final var by1 = calibrator.getBiasYAsAcceleration();
5921         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
5922         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
5923         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5924         calibrator.getBiasYAsAcceleration(by2);
5925         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
5926         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
5927         final var bz1 = calibrator.getBiasZAsAcceleration();
5928         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
5929         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
5930         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
5931         calibrator.getBiasZAsAcceleration(bz2);
5932         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
5933         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
5934         final var biasTriad1 = calibrator.getBiasAsTriad();
5935         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
5936         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
5937         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
5938         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
5939         final var biasTriad2 = new AccelerationTriad();
5940         calibrator.getBiasAsTriad(biasTriad2);
5941         assertEquals(biasTriad1, biasTriad2);
5942         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
5943         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
5944         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
5945         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
5946         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
5947         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
5948         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
5949         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
5950         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
5951         final var bias1 = calibrator.getBias();
5952         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5953         final var bias2 = new double[3];
5954         calibrator.getBias(bias2);
5955         assertArrayEquals(bias1, bias2, 0.0);
5956         final var b1 = calibrator.getBiasAsMatrix();
5957         assertEquals(b1, ba);
5958         final var b2 = new Matrix(3, 1);
5959         calibrator.getBiasAsMatrix(b2);
5960         assertEquals(b1, b2);
5961         final var ma1 = calibrator.getInitialMa();
5962         assertEquals(new Matrix(3, 3), ma1);
5963         final var ma2 = new Matrix(3, 3);
5964         calibrator.getInitialMa(ma2);
5965         assertEquals(ma1, ma2);
5966         assertSame(measurements, calibrator.getMeasurements());
5967         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
5968                 calibrator.getMeasurementType());
5969         assertFalse(calibrator.isOrderedMeasurementsRequired());
5970         assertFalse(calibrator.isQualityScoresRequired());
5971         assertFalse(calibrator.isCommonAxisUsed());
5972         assertSame(this, calibrator.getListener());
5973         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
5974         assertFalse(calibrator.isReady());
5975         assertFalse(calibrator.isRunning());
5976         assertNull(calibrator.getEstimatedMa());
5977         assertNull(calibrator.getEstimatedSx());
5978         assertNull(calibrator.getEstimatedSy());
5979         assertNull(calibrator.getEstimatedSz());
5980         assertNull(calibrator.getEstimatedMxy());
5981         assertNull(calibrator.getEstimatedMxz());
5982         assertNull(calibrator.getEstimatedMyx());
5983         assertNull(calibrator.getEstimatedMyz());
5984         assertNull(calibrator.getEstimatedMzx());
5985         assertNull(calibrator.getEstimatedMzy());
5986         assertNull(calibrator.getEstimatedCovariance());
5987         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
5988         assertNull(calibrator.getGroundTruthGravityNorm());
5989         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5990         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5991         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
5992 
5993         // Force IllegalArgumentException
5994         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
5995                 measurements, new double[1], this));
5996     }
5997 
5998     @Test
5999     void testConstructor59() throws WrongSizeException {
6000         final var ba = generateBa();
6001         final var bias = ba.getBuffer();
6002         final var biasX = ba.getElementAtIndex(0);
6003         final var biasY = ba.getElementAtIndex(1);
6004         final var biasZ = ba.getElementAtIndex(2);
6005 
6006         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bias);
6007 
6008         // check default values
6009         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6010         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6011         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6012         final var bx1 = calibrator.getBiasXAsAcceleration();
6013         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6014         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6015         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6016         calibrator.getBiasXAsAcceleration(bx2);
6017         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6018         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6019         final var by1 = calibrator.getBiasYAsAcceleration();
6020         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6021         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6022         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6023         calibrator.getBiasYAsAcceleration(by2);
6024         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6025         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6026         final var bz1 = calibrator.getBiasZAsAcceleration();
6027         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6028         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6029         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6030         calibrator.getBiasZAsAcceleration(bz2);
6031         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6032         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6033         final var biasTriad1 = calibrator.getBiasAsTriad();
6034         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6035         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6036         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6037         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6038         final var biasTriad2 = new AccelerationTriad();
6039         calibrator.getBiasAsTriad(biasTriad2);
6040         assertEquals(biasTriad1, biasTriad2);
6041         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6042         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6043         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6044         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6045         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6046         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6047         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6048         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6049         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6050         final var bias1 = calibrator.getBias();
6051         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6052         final var bias2 = new double[3];
6053         calibrator.getBias(bias2);
6054         assertArrayEquals(bias1, bias2, 0.0);
6055         final var b1 = calibrator.getBiasAsMatrix();
6056         assertEquals(b1, ba);
6057         final var b2 = new Matrix(3, 1);
6058         calibrator.getBiasAsMatrix(b2);
6059         assertEquals(b1, b2);
6060         final var ma1 = calibrator.getInitialMa();
6061         assertEquals(new Matrix(3, 3), ma1);
6062         final var ma2 = new Matrix(3, 3);
6063         calibrator.getInitialMa(ma2);
6064         assertEquals(ma1, ma2);
6065         assertNull(calibrator.getMeasurements());
6066         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6067                 calibrator.getMeasurementType());
6068         assertFalse(calibrator.isOrderedMeasurementsRequired());
6069         assertFalse(calibrator.isQualityScoresRequired());
6070         assertTrue(calibrator.isCommonAxisUsed());
6071         assertNull(calibrator.getListener());
6072         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6073         assertFalse(calibrator.isReady());
6074         assertFalse(calibrator.isRunning());
6075         assertNull(calibrator.getEstimatedMa());
6076         assertNull(calibrator.getEstimatedSx());
6077         assertNull(calibrator.getEstimatedSy());
6078         assertNull(calibrator.getEstimatedSz());
6079         assertNull(calibrator.getEstimatedMxy());
6080         assertNull(calibrator.getEstimatedMxz());
6081         assertNull(calibrator.getEstimatedMyx());
6082         assertNull(calibrator.getEstimatedMyz());
6083         assertNull(calibrator.getEstimatedMzx());
6084         assertNull(calibrator.getEstimatedMzy());
6085         assertNull(calibrator.getEstimatedCovariance());
6086         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6087         assertNull(calibrator.getGroundTruthGravityNorm());
6088         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6089         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6090         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6091 
6092         // Force IllegalArgumentException
6093         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(true,
6094                 new double[1]));
6095     }
6096 
6097     @Test
6098     void testConstructor60() throws WrongSizeException {
6099         final var ba = generateBa();
6100         final var bias = ba.getBuffer();
6101         final var biasX = ba.getElementAtIndex(0);
6102         final var biasY = ba.getElementAtIndex(1);
6103         final var biasZ = ba.getElementAtIndex(2);
6104 
6105         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, bias,
6106                 this);
6107 
6108         // check default values
6109         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6110         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6111         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6112         final var bx1 = calibrator.getBiasXAsAcceleration();
6113         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6114         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6115         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6116         calibrator.getBiasXAsAcceleration(bx2);
6117         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6118         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6119         final var by1 = calibrator.getBiasYAsAcceleration();
6120         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6121         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6122         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6123         calibrator.getBiasYAsAcceleration(by2);
6124         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6125         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6126         final var bz1 = calibrator.getBiasZAsAcceleration();
6127         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6128         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6129         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6130         calibrator.getBiasZAsAcceleration(bz2);
6131         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6132         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6133         final var biasTriad1 = calibrator.getBiasAsTriad();
6134         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6135         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6136         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6137         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6138         final var biasTriad2 = new AccelerationTriad();
6139         calibrator.getBiasAsTriad(biasTriad2);
6140         assertEquals(biasTriad1, biasTriad2);
6141         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6142         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6143         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6144         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6145         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6146         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6147         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6148         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6149         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6150         final var bias1 = calibrator.getBias();
6151         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6152         final var bias2 = new double[3];
6153         calibrator.getBias(bias2);
6154         assertArrayEquals(bias1, bias2, 0.0);
6155         final var b1 = calibrator.getBiasAsMatrix();
6156         assertEquals(b1, ba);
6157         final var b2 = new Matrix(3, 1);
6158         calibrator.getBiasAsMatrix(b2);
6159         assertEquals(b1, b2);
6160         final var ma1 = calibrator.getInitialMa();
6161         assertEquals(new Matrix(3, 3), ma1);
6162         final var ma2 = new Matrix(3, 3);
6163         calibrator.getInitialMa(ma2);
6164         assertEquals(ma1, ma2);
6165         assertNull(calibrator.getMeasurements());
6166         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6167                 calibrator.getMeasurementType());
6168         assertFalse(calibrator.isOrderedMeasurementsRequired());
6169         assertFalse(calibrator.isQualityScoresRequired());
6170         assertTrue(calibrator.isCommonAxisUsed());
6171         assertSame(this, calibrator.getListener());
6172         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6173         assertFalse(calibrator.isReady());
6174         assertFalse(calibrator.isRunning());
6175         assertNull(calibrator.getEstimatedMa());
6176         assertNull(calibrator.getEstimatedSx());
6177         assertNull(calibrator.getEstimatedSy());
6178         assertNull(calibrator.getEstimatedSz());
6179         assertNull(calibrator.getEstimatedMxy());
6180         assertNull(calibrator.getEstimatedMxz());
6181         assertNull(calibrator.getEstimatedMyx());
6182         assertNull(calibrator.getEstimatedMyz());
6183         assertNull(calibrator.getEstimatedMzx());
6184         assertNull(calibrator.getEstimatedMzy());
6185         assertNull(calibrator.getEstimatedCovariance());
6186         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6187         assertNull(calibrator.getGroundTruthGravityNorm());
6188         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6189         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6190         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6191 
6192         // Force IllegalArgumentException
6193         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6194                 true, new double[1], this));
6195     }
6196 
6197     @Test
6198     void testConstructor61() throws WrongSizeException {
6199         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6200 
6201         final var ba = generateBa();
6202         final var bias = ba.getBuffer();
6203         final var biasX = ba.getElementAtIndex(0);
6204         final var biasY = ba.getElementAtIndex(1);
6205         final var biasZ = ba.getElementAtIndex(2);
6206 
6207         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
6208                 bias);
6209 
6210         // check default values
6211         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6212         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6213         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6214         final var bx1 = calibrator.getBiasXAsAcceleration();
6215         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6216         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6217         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6218         calibrator.getBiasXAsAcceleration(bx2);
6219         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6220         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6221         final var by1 = calibrator.getBiasYAsAcceleration();
6222         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6223         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6224         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6225         calibrator.getBiasYAsAcceleration(by2);
6226         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6227         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6228         final var bz1 = calibrator.getBiasZAsAcceleration();
6229         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6230         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6231         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6232         calibrator.getBiasZAsAcceleration(bz2);
6233         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6234         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6235         final var biasTriad1 = calibrator.getBiasAsTriad();
6236         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6237         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6238         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6239         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6240         final var biasTriad2 = new AccelerationTriad();
6241         calibrator.getBiasAsTriad(biasTriad2);
6242         assertEquals(biasTriad1, biasTriad2);
6243         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6244         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6245         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6246         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6247         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6248         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6249         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6250         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6251         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6252         final var bias1 = calibrator.getBias();
6253         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6254         final var bias2 = new double[3];
6255         calibrator.getBias(bias2);
6256         assertArrayEquals(bias1, bias2, 0.0);
6257         final var b1 = calibrator.getBiasAsMatrix();
6258         assertEquals(b1, ba);
6259         final var b2 = new Matrix(3, 1);
6260         calibrator.getBiasAsMatrix(b2);
6261         assertEquals(b1, b2);
6262         final var ma1 = calibrator.getInitialMa();
6263         assertEquals(new Matrix(3, 3), ma1);
6264         final var ma2 = new Matrix(3, 3);
6265         calibrator.getInitialMa(ma2);
6266         assertEquals(ma1, ma2);
6267         assertSame(measurements, calibrator.getMeasurements());
6268         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6269                 calibrator.getMeasurementType());
6270         assertFalse(calibrator.isOrderedMeasurementsRequired());
6271         assertFalse(calibrator.isQualityScoresRequired());
6272         assertTrue(calibrator.isCommonAxisUsed());
6273         assertNull(calibrator.getListener());
6274         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6275         assertFalse(calibrator.isReady());
6276         assertFalse(calibrator.isRunning());
6277         assertNull(calibrator.getEstimatedMa());
6278         assertNull(calibrator.getEstimatedSx());
6279         assertNull(calibrator.getEstimatedSy());
6280         assertNull(calibrator.getEstimatedSz());
6281         assertNull(calibrator.getEstimatedMxy());
6282         assertNull(calibrator.getEstimatedMxz());
6283         assertNull(calibrator.getEstimatedMyx());
6284         assertNull(calibrator.getEstimatedMyz());
6285         assertNull(calibrator.getEstimatedMzx());
6286         assertNull(calibrator.getEstimatedMzy());
6287         assertNull(calibrator.getEstimatedCovariance());
6288         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6289         assertNull(calibrator.getGroundTruthGravityNorm());
6290         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6291         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6292         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6293 
6294         // Force IllegalArgumentException
6295         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6296                 measurements, true, new double[1]));
6297     }
6298 
6299     @Test
6300     void testConstructor62() throws WrongSizeException {
6301         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6302 
6303         final var ba = generateBa();
6304         final var bias = ba.getBuffer();
6305         final var biasX = ba.getElementAtIndex(0);
6306         final var biasY = ba.getElementAtIndex(1);
6307         final var biasZ = ba.getElementAtIndex(2);
6308 
6309         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
6310                 bias, this);
6311 
6312         // check default values
6313         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6314         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6315         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6316         final var bx1 = calibrator.getBiasXAsAcceleration();
6317         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6318         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6319         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6320         calibrator.getBiasXAsAcceleration(bx2);
6321         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6322         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6323         final var by1 = calibrator.getBiasYAsAcceleration();
6324         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6325         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6326         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6327         calibrator.getBiasYAsAcceleration(by2);
6328         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6329         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6330         final var bz1 = calibrator.getBiasZAsAcceleration();
6331         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6332         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6333         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6334         calibrator.getBiasZAsAcceleration(bz2);
6335         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6336         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6337         final var biasTriad1 = calibrator.getBiasAsTriad();
6338         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6339         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6340         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6341         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6342         final var biasTriad2 = new AccelerationTriad();
6343         calibrator.getBiasAsTriad(biasTriad2);
6344         assertEquals(biasTriad1, biasTriad2);
6345         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6346         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6347         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6348         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6349         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6350         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6351         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6352         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6353         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6354         final var bias1 = calibrator.getBias();
6355         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6356         final var bias2 = new double[3];
6357         calibrator.getBias(bias2);
6358         assertArrayEquals(bias1, bias2, 0.0);
6359         final var b1 = calibrator.getBiasAsMatrix();
6360         assertEquals(b1, ba);
6361         final var b2 = new Matrix(3, 1);
6362         calibrator.getBiasAsMatrix(b2);
6363         assertEquals(b1, b2);
6364         final var ma1 = calibrator.getInitialMa();
6365         assertEquals(new Matrix(3, 3), ma1);
6366         final var ma2 = new Matrix(3, 3);
6367         calibrator.getInitialMa(ma2);
6368         assertEquals(ma1, ma2);
6369         assertSame(measurements, calibrator.getMeasurements());
6370         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6371                 calibrator.getMeasurementType());
6372         assertFalse(calibrator.isOrderedMeasurementsRequired());
6373         assertFalse(calibrator.isQualityScoresRequired());
6374         assertTrue(calibrator.isCommonAxisUsed());
6375         assertSame(this, calibrator.getListener());
6376         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6377         assertFalse(calibrator.isReady());
6378         assertFalse(calibrator.isRunning());
6379         assertNull(calibrator.getEstimatedMa());
6380         assertNull(calibrator.getEstimatedSx());
6381         assertNull(calibrator.getEstimatedSy());
6382         assertNull(calibrator.getEstimatedSz());
6383         assertNull(calibrator.getEstimatedMxy());
6384         assertNull(calibrator.getEstimatedMxz());
6385         assertNull(calibrator.getEstimatedMyx());
6386         assertNull(calibrator.getEstimatedMyz());
6387         assertNull(calibrator.getEstimatedMzx());
6388         assertNull(calibrator.getEstimatedMzy());
6389         assertNull(calibrator.getEstimatedCovariance());
6390         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6391         assertNull(calibrator.getGroundTruthGravityNorm());
6392         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6393         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6394         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6395 
6396         // Force IllegalArgumentException
6397         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6398                 measurements, true, new double[1], this));
6399     }
6400 
6401     @Test
6402     void testConstructor63() throws WrongSizeException {
6403         final var ba = generateBa();
6404         final var biasX = ba.getElementAtIndex(0);
6405         final var biasY = ba.getElementAtIndex(1);
6406         final var biasZ = ba.getElementAtIndex(2);
6407 
6408         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba);
6409 
6410         // check default values
6411         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6412         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6413         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6414         final var bx1 = calibrator.getBiasXAsAcceleration();
6415         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6416         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6417         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6418         calibrator.getBiasXAsAcceleration(bx2);
6419         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6420         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6421         final var by1 = calibrator.getBiasYAsAcceleration();
6422         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6423         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6424         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6425         calibrator.getBiasYAsAcceleration(by2);
6426         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6427         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6428         final var bz1 = calibrator.getBiasZAsAcceleration();
6429         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6430         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6431         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6432         calibrator.getBiasZAsAcceleration(bz2);
6433         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6434         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6435         final var biasTriad1 = calibrator.getBiasAsTriad();
6436         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6437         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6438         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6439         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6440         final var biasTriad2 = new AccelerationTriad();
6441         calibrator.getBiasAsTriad(biasTriad2);
6442         assertEquals(biasTriad1, biasTriad2);
6443         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6444         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6445         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6446         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6447         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6448         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6449         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6450         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6451         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6452         final var bias1 = calibrator.getBias();
6453         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6454         final var bias2 = new double[3];
6455         calibrator.getBias(bias2);
6456         assertArrayEquals(bias1, bias2, 0.0);
6457         final var b1 = calibrator.getBiasAsMatrix();
6458         assertEquals(b1, ba);
6459         final var b2 = new Matrix(3, 1);
6460         calibrator.getBiasAsMatrix(b2);
6461         assertEquals(b1, b2);
6462         final var ma1 = calibrator.getInitialMa();
6463         assertEquals(new Matrix(3, 3), ma1);
6464         final var ma2 = new Matrix(3, 3);
6465         calibrator.getInitialMa(ma2);
6466         assertEquals(ma1, ma2);
6467         assertNull(calibrator.getMeasurements());
6468         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6469                 calibrator.getMeasurementType());
6470         assertFalse(calibrator.isOrderedMeasurementsRequired());
6471         assertFalse(calibrator.isQualityScoresRequired());
6472         assertFalse(calibrator.isCommonAxisUsed());
6473         assertNull(calibrator.getListener());
6474         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6475         assertFalse(calibrator.isReady());
6476         assertFalse(calibrator.isRunning());
6477         assertNull(calibrator.getEstimatedMa());
6478         assertNull(calibrator.getEstimatedSx());
6479         assertNull(calibrator.getEstimatedSy());
6480         assertNull(calibrator.getEstimatedSz());
6481         assertNull(calibrator.getEstimatedMxy());
6482         assertNull(calibrator.getEstimatedMxz());
6483         assertNull(calibrator.getEstimatedMyx());
6484         assertNull(calibrator.getEstimatedMyz());
6485         assertNull(calibrator.getEstimatedMzx());
6486         assertNull(calibrator.getEstimatedMzy());
6487         assertNull(calibrator.getEstimatedCovariance());
6488         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6489         assertNull(calibrator.getGroundTruthGravityNorm());
6490         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6491         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6492         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6493 
6494         // Force IllegalArgumentException
6495         final var m1 = new Matrix(1, 1);
6496         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m1));
6497         final var m2 = new Matrix(1, 3);
6498         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m2));
6499     }
6500 
6501     @Test
6502     void testConstructor64() throws WrongSizeException {
6503         final var ba = generateBa();
6504         final var biasX = ba.getElementAtIndex(0);
6505         final var biasY = ba.getElementAtIndex(1);
6506         final var biasZ = ba.getElementAtIndex(2);
6507 
6508         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba, this);
6509 
6510         // check default values
6511         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6512         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6513         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6514         final var bx1 = calibrator.getBiasXAsAcceleration();
6515         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6516         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6517         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6518         calibrator.getBiasXAsAcceleration(bx2);
6519         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6520         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6521         final var by1 = calibrator.getBiasYAsAcceleration();
6522         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6523         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6524         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6525         calibrator.getBiasYAsAcceleration(by2);
6526         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6527         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6528         final var bz1 = calibrator.getBiasZAsAcceleration();
6529         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6530         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6531         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6532         calibrator.getBiasZAsAcceleration(bz2);
6533         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6534         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6535         final var biasTriad1 = calibrator.getBiasAsTriad();
6536         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6537         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6538         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6539         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6540         final var biasTriad2 = new AccelerationTriad();
6541         calibrator.getBiasAsTriad(biasTriad2);
6542         assertEquals(biasTriad1, biasTriad2);
6543         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6544         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6545         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6546         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6547         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6548         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6549         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6550         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6551         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6552         final var bias1 = calibrator.getBias();
6553         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6554         final var bias2 = new double[3];
6555         calibrator.getBias(bias2);
6556         assertArrayEquals(bias1, bias2, 0.0);
6557         final var b1 = calibrator.getBiasAsMatrix();
6558         assertEquals(b1, ba);
6559         final var b2 = new Matrix(3, 1);
6560         calibrator.getBiasAsMatrix(b2);
6561         assertEquals(b1, b2);
6562         final var ma1 = calibrator.getInitialMa();
6563         assertEquals(new Matrix(3, 3), ma1);
6564         final var ma2 = new Matrix(3, 3);
6565         calibrator.getInitialMa(ma2);
6566         assertEquals(ma1, ma2);
6567         assertNull(calibrator.getMeasurements());
6568         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6569                 calibrator.getMeasurementType());
6570         assertFalse(calibrator.isOrderedMeasurementsRequired());
6571         assertFalse(calibrator.isQualityScoresRequired());
6572         assertFalse(calibrator.isCommonAxisUsed());
6573         assertSame(this, calibrator.getListener());
6574         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6575         assertFalse(calibrator.isReady());
6576         assertFalse(calibrator.isRunning());
6577         assertNull(calibrator.getEstimatedMa());
6578         assertNull(calibrator.getEstimatedSx());
6579         assertNull(calibrator.getEstimatedSy());
6580         assertNull(calibrator.getEstimatedSz());
6581         assertNull(calibrator.getEstimatedMxy());
6582         assertNull(calibrator.getEstimatedMxz());
6583         assertNull(calibrator.getEstimatedMyx());
6584         assertNull(calibrator.getEstimatedMyz());
6585         assertNull(calibrator.getEstimatedMzx());
6586         assertNull(calibrator.getEstimatedMzy());
6587         assertNull(calibrator.getEstimatedCovariance());
6588         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6589         assertNull(calibrator.getGroundTruthGravityNorm());
6590         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6591         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6592         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6593 
6594         // Force IllegalArgumentException
6595         final var m1 = new Matrix(1, 1);
6596         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m1));
6597         final var m2 = new Matrix(1, 3);
6598         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m2));
6599     }
6600 
6601     @Test
6602     void testConstructor65() throws WrongSizeException {
6603         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6604 
6605         final var ba = generateBa();
6606         final var biasX = ba.getElementAtIndex(0);
6607         final var biasY = ba.getElementAtIndex(1);
6608         final var biasZ = ba.getElementAtIndex(2);
6609 
6610         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba);
6611 
6612         // check default values
6613         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6614         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6615         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6616         final var bx1 = calibrator.getBiasXAsAcceleration();
6617         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6618         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6619         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6620         calibrator.getBiasXAsAcceleration(bx2);
6621         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6622         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6623         final var by1 = calibrator.getBiasYAsAcceleration();
6624         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6625         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6626         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6627         calibrator.getBiasYAsAcceleration(by2);
6628         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6629         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6630         final var bz1 = calibrator.getBiasZAsAcceleration();
6631         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6632         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6633         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6634         calibrator.getBiasZAsAcceleration(bz2);
6635         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6636         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6637         final var biasTriad1 = calibrator.getBiasAsTriad();
6638         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6639         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6640         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6641         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6642         final var biasTriad2 = new AccelerationTriad();
6643         calibrator.getBiasAsTriad(biasTriad2);
6644         assertEquals(biasTriad1, biasTriad2);
6645         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6646         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6647         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6648         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6649         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6650         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6651         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6652         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6653         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6654         final var bias1 = calibrator.getBias();
6655         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6656         final var bias2 = new double[3];
6657         calibrator.getBias(bias2);
6658         assertArrayEquals(bias1, bias2, 0.0);
6659         final var b1 = calibrator.getBiasAsMatrix();
6660         assertEquals(b1, ba);
6661         final var b2 = new Matrix(3, 1);
6662         calibrator.getBiasAsMatrix(b2);
6663         assertEquals(b1, b2);
6664         final var ma1 = calibrator.getInitialMa();
6665         assertEquals(new Matrix(3, 3), ma1);
6666         final var ma2 = new Matrix(3, 3);
6667         calibrator.getInitialMa(ma2);
6668         assertEquals(ma1, ma2);
6669         assertSame(measurements, calibrator.getMeasurements());
6670         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6671                 calibrator.getMeasurementType());
6672         assertFalse(calibrator.isOrderedMeasurementsRequired());
6673         assertFalse(calibrator.isQualityScoresRequired());
6674         assertFalse(calibrator.isCommonAxisUsed());
6675         assertNull(calibrator.getListener());
6676         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6677         assertFalse(calibrator.isReady());
6678         assertFalse(calibrator.isRunning());
6679         assertNull(calibrator.getEstimatedMa());
6680         assertNull(calibrator.getEstimatedSx());
6681         assertNull(calibrator.getEstimatedSy());
6682         assertNull(calibrator.getEstimatedSz());
6683         assertNull(calibrator.getEstimatedMxy());
6684         assertNull(calibrator.getEstimatedMxz());
6685         assertNull(calibrator.getEstimatedMyx());
6686         assertNull(calibrator.getEstimatedMyz());
6687         assertNull(calibrator.getEstimatedMzx());
6688         assertNull(calibrator.getEstimatedMzy());
6689         assertNull(calibrator.getEstimatedCovariance());
6690         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6691         assertNull(calibrator.getGroundTruthGravityNorm());
6692         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6693         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6694         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6695 
6696         // Force IllegalArgumentException
6697         final var m1 = new Matrix(1, 1);
6698         assertThrows(IllegalArgumentException.class,
6699                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, m1));
6700         final var m2 = new Matrix(1, 3);
6701         assertThrows(IllegalArgumentException.class,
6702                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, m2));
6703     }
6704 
6705     @Test
6706     void testConstructor66() throws WrongSizeException {
6707         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
6708 
6709         final var ba = generateBa();
6710         final var biasX = ba.getElementAtIndex(0);
6711         final var biasY = ba.getElementAtIndex(1);
6712         final var biasZ = ba.getElementAtIndex(2);
6713 
6714         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba, this);
6715 
6716         // check default values
6717         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6718         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6719         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6720         final var bx1 = calibrator.getBiasXAsAcceleration();
6721         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6722         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6723         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6724         calibrator.getBiasXAsAcceleration(bx2);
6725         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6726         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6727         final var by1 = calibrator.getBiasYAsAcceleration();
6728         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6729         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6730         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6731         calibrator.getBiasYAsAcceleration(by2);
6732         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6733         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6734         final var bz1 = calibrator.getBiasZAsAcceleration();
6735         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6736         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6737         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6738         calibrator.getBiasZAsAcceleration(bz2);
6739         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6740         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6741         final var biasTriad1 = calibrator.getBiasAsTriad();
6742         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6743         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6744         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6745         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6746         final var biasTriad2 = new AccelerationTriad();
6747         calibrator.getBiasAsTriad(biasTriad2);
6748         assertEquals(biasTriad1, biasTriad2);
6749         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6750         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6751         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6752         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6753         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6754         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6755         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6756         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6757         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6758         final var bias1 = calibrator.getBias();
6759         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6760         final var bias2 = new double[3];
6761         calibrator.getBias(bias2);
6762         assertArrayEquals(bias1, bias2, 0.0);
6763         final var b1 = calibrator.getBiasAsMatrix();
6764         assertEquals(b1, ba);
6765         final var b2 = new Matrix(3, 1);
6766         calibrator.getBiasAsMatrix(b2);
6767         assertEquals(b1, b2);
6768         final var ma1 = calibrator.getInitialMa();
6769         assertEquals(new Matrix(3, 3), ma1);
6770         final var ma2 = new Matrix(3, 3);
6771         calibrator.getInitialMa(ma2);
6772         assertEquals(ma1, ma2);
6773         assertSame(measurements, calibrator.getMeasurements());
6774         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6775                 calibrator.getMeasurementType());
6776         assertFalse(calibrator.isOrderedMeasurementsRequired());
6777         assertFalse(calibrator.isQualityScoresRequired());
6778         assertFalse(calibrator.isCommonAxisUsed());
6779         assertSame(this, calibrator.getListener());
6780         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
6781         assertFalse(calibrator.isReady());
6782         assertFalse(calibrator.isRunning());
6783         assertNull(calibrator.getEstimatedMa());
6784         assertNull(calibrator.getEstimatedSx());
6785         assertNull(calibrator.getEstimatedSy());
6786         assertNull(calibrator.getEstimatedSz());
6787         assertNull(calibrator.getEstimatedMxy());
6788         assertNull(calibrator.getEstimatedMxz());
6789         assertNull(calibrator.getEstimatedMyx());
6790         assertNull(calibrator.getEstimatedMyz());
6791         assertNull(calibrator.getEstimatedMzx());
6792         assertNull(calibrator.getEstimatedMzy());
6793         assertNull(calibrator.getEstimatedCovariance());
6794         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6795         assertNull(calibrator.getGroundTruthGravityNorm());
6796         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6797         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6798         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6799 
6800         // Force IllegalArgumentException
6801         final var m1 = new Matrix(1, 1);
6802         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6803                 measurements, m1, this));
6804         final var m2 = new Matrix(1, 3);
6805         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6806                 measurements, m2, this));
6807     }
6808 
6809     @Test
6810     void testConstructor67() throws WrongSizeException {
6811         final var ba = generateBa();
6812         final var biasX = ba.getElementAtIndex(0);
6813         final var biasY = ba.getElementAtIndex(1);
6814         final var biasZ = ba.getElementAtIndex(2);
6815 
6816         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba);
6817 
6818         // check default values
6819         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6820         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6821         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6822         final var bx1 = calibrator.getBiasXAsAcceleration();
6823         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6824         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6825         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6826         calibrator.getBiasXAsAcceleration(bx2);
6827         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6828         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6829         final var by1 = calibrator.getBiasYAsAcceleration();
6830         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6831         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6832         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6833         calibrator.getBiasYAsAcceleration(by2);
6834         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6835         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6836         final var bz1 = calibrator.getBiasZAsAcceleration();
6837         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6838         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6839         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6840         calibrator.getBiasZAsAcceleration(bz2);
6841         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6842         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6843         final var biasTriad1 = calibrator.getBiasAsTriad();
6844         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6845         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6846         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6847         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6848         final var biasTriad2 = new AccelerationTriad();
6849         calibrator.getBiasAsTriad(biasTriad2);
6850         assertEquals(biasTriad1, biasTriad2);
6851         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6852         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6853         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6854         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6855         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6856         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6857         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6858         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6859         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6860         final var bias1 = calibrator.getBias();
6861         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6862         final var bias2 = new double[3];
6863         calibrator.getBias(bias2);
6864         assertArrayEquals(bias1, bias2, 0.0);
6865         final var b1 = calibrator.getBiasAsMatrix();
6866         assertEquals(b1, ba);
6867         final var b2 = new Matrix(3, 1);
6868         calibrator.getBiasAsMatrix(b2);
6869         assertEquals(b1, b2);
6870         final var ma1 = calibrator.getInitialMa();
6871         assertEquals(new Matrix(3, 3), ma1);
6872         final var ma2 = new Matrix(3, 3);
6873         calibrator.getInitialMa(ma2);
6874         assertEquals(ma1, ma2);
6875         assertNull(calibrator.getMeasurements());
6876         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6877                 calibrator.getMeasurementType());
6878         assertFalse(calibrator.isOrderedMeasurementsRequired());
6879         assertFalse(calibrator.isQualityScoresRequired());
6880         assertTrue(calibrator.isCommonAxisUsed());
6881         assertNull(calibrator.getListener());
6882         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6883         assertFalse(calibrator.isReady());
6884         assertFalse(calibrator.isRunning());
6885         assertNull(calibrator.getEstimatedMa());
6886         assertNull(calibrator.getEstimatedSx());
6887         assertNull(calibrator.getEstimatedSy());
6888         assertNull(calibrator.getEstimatedSz());
6889         assertNull(calibrator.getEstimatedMxy());
6890         assertNull(calibrator.getEstimatedMxz());
6891         assertNull(calibrator.getEstimatedMyx());
6892         assertNull(calibrator.getEstimatedMyz());
6893         assertNull(calibrator.getEstimatedMzx());
6894         assertNull(calibrator.getEstimatedMzy());
6895         assertNull(calibrator.getEstimatedCovariance());
6896         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
6897         assertNull(calibrator.getGroundTruthGravityNorm());
6898         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6899         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6900         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
6901 
6902         // Force IllegalArgumentException
6903         final var m1 = new Matrix(1, 1);
6904         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6905                 true, m1));
6906         final var m2 = new Matrix(1, 3);
6907         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
6908                 true, m2));
6909     }
6910 
6911     @Test
6912     void testConstructor68() throws WrongSizeException {
6913         final var ba = generateBa();
6914         final var biasX = ba.getElementAtIndex(0);
6915         final var biasY = ba.getElementAtIndex(1);
6916         final var biasZ = ba.getElementAtIndex(2);
6917 
6918         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba,
6919                 this);
6920 
6921         // check default values
6922         assertEquals(biasX, calibrator.getBiasX(), 0.0);
6923         assertEquals(biasY, calibrator.getBiasY(), 0.0);
6924         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
6925         final var bx1 = calibrator.getBiasXAsAcceleration();
6926         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
6927         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
6928         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6929         calibrator.getBiasXAsAcceleration(bx2);
6930         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
6931         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
6932         final var by1 = calibrator.getBiasYAsAcceleration();
6933         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
6934         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
6935         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6936         calibrator.getBiasYAsAcceleration(by2);
6937         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
6938         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
6939         final var bz1 = calibrator.getBiasZAsAcceleration();
6940         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
6941         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
6942         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
6943         calibrator.getBiasZAsAcceleration(bz2);
6944         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
6945         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
6946         final var biasTriad1 = calibrator.getBiasAsTriad();
6947         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
6948         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
6949         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
6950         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
6951         final var biasTriad2 = new AccelerationTriad();
6952         calibrator.getBiasAsTriad(biasTriad2);
6953         assertEquals(biasTriad1, biasTriad2);
6954         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
6955         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
6956         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
6957         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
6958         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
6959         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
6960         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
6961         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
6962         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
6963         final var bias1 = calibrator.getBias();
6964         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6965         final var bias2 = new double[3];
6966         calibrator.getBias(bias2);
6967         assertArrayEquals(bias1, bias2, 0.0);
6968         final var b1 = calibrator.getBiasAsMatrix();
6969         assertEquals(b1, ba);
6970         final var b2 = new Matrix(3, 1);
6971         calibrator.getBiasAsMatrix(b2);
6972         assertEquals(b1, b2);
6973         final var ma1 = calibrator.getInitialMa();
6974         assertEquals(new Matrix(3, 3), ma1);
6975         final var ma2 = new Matrix(3, 3);
6976         calibrator.getInitialMa(ma2);
6977         assertEquals(ma1, ma2);
6978         assertNull(calibrator.getMeasurements());
6979         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
6980                 calibrator.getMeasurementType());
6981         assertFalse(calibrator.isOrderedMeasurementsRequired());
6982         assertFalse(calibrator.isQualityScoresRequired());
6983         assertTrue(calibrator.isCommonAxisUsed());
6984         assertSame(this, calibrator.getListener());
6985         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
6986         assertFalse(calibrator.isReady());
6987         assertFalse(calibrator.isRunning());
6988         assertNull(calibrator.getEstimatedMa());
6989         assertNull(calibrator.getEstimatedSx());
6990         assertNull(calibrator.getEstimatedSy());
6991         assertNull(calibrator.getEstimatedSz());
6992         assertNull(calibrator.getEstimatedMxy());
6993         assertNull(calibrator.getEstimatedMxz());
6994         assertNull(calibrator.getEstimatedMyx());
6995         assertNull(calibrator.getEstimatedMyz());
6996         assertNull(calibrator.getEstimatedMzx());
6997         assertNull(calibrator.getEstimatedMzy());
6998         assertNull(calibrator.getEstimatedCovariance());
6999         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7000         assertNull(calibrator.getGroundTruthGravityNorm());
7001         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7002         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7003         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7004 
7005         // Force IllegalArgumentException
7006         final var m1 = new Matrix(1, 1);
7007         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7008                 true, m1, this));
7009         final var m2 = new Matrix(1, 3);
7010         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7011                 true, m2, this));
7012     }
7013 
7014     @Test
7015     void testConstructor69() throws WrongSizeException {
7016         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7017 
7018         final var ba = generateBa();
7019         final var biasX = ba.getElementAtIndex(0);
7020         final var biasY = ba.getElementAtIndex(1);
7021         final var biasZ = ba.getElementAtIndex(2);
7022 
7023         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
7024                 ba);
7025 
7026         // check default values
7027         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7028         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7029         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7030         final var bx1 = calibrator.getBiasXAsAcceleration();
7031         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7032         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7033         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7034         calibrator.getBiasXAsAcceleration(bx2);
7035         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7036         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7037         final var by1 = calibrator.getBiasYAsAcceleration();
7038         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7039         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7040         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7041         calibrator.getBiasYAsAcceleration(by2);
7042         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7043         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7044         final var bz1 = calibrator.getBiasZAsAcceleration();
7045         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7046         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7047         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7048         calibrator.getBiasZAsAcceleration(bz2);
7049         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7050         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7051         final var biasTriad1 = calibrator.getBiasAsTriad();
7052         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7053         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7054         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7055         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7056         final var biasTriad2 = new AccelerationTriad();
7057         calibrator.getBiasAsTriad(biasTriad2);
7058         assertEquals(biasTriad1, biasTriad2);
7059         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
7060         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
7061         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
7062         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
7063         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
7064         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
7065         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
7066         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
7067         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
7068         final var bias1 = calibrator.getBias();
7069         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7070         final var bias2 = new double[3];
7071         calibrator.getBias(bias2);
7072         assertArrayEquals(bias1, bias2, 0.0);
7073         final var b1 = calibrator.getBiasAsMatrix();
7074         assertEquals(b1, ba);
7075         final var b2 = new Matrix(3, 1);
7076         calibrator.getBiasAsMatrix(b2);
7077         assertEquals(b1, b2);
7078         final var ma1 = calibrator.getInitialMa();
7079         assertEquals(new Matrix(3, 3), ma1);
7080         final var ma2 = new Matrix(3, 3);
7081         calibrator.getInitialMa(ma2);
7082         assertEquals(ma1, ma2);
7083         assertSame(measurements, calibrator.getMeasurements());
7084         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7085                 calibrator.getMeasurementType());
7086         assertFalse(calibrator.isOrderedMeasurementsRequired());
7087         assertFalse(calibrator.isQualityScoresRequired());
7088         assertTrue(calibrator.isCommonAxisUsed());
7089         assertNull(calibrator.getListener());
7090         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7091         assertFalse(calibrator.isReady());
7092         assertFalse(calibrator.isRunning());
7093         assertNull(calibrator.getEstimatedMa());
7094         assertNull(calibrator.getEstimatedSx());
7095         assertNull(calibrator.getEstimatedSy());
7096         assertNull(calibrator.getEstimatedSz());
7097         assertNull(calibrator.getEstimatedMxy());
7098         assertNull(calibrator.getEstimatedMxz());
7099         assertNull(calibrator.getEstimatedMyx());
7100         assertNull(calibrator.getEstimatedMyz());
7101         assertNull(calibrator.getEstimatedMzx());
7102         assertNull(calibrator.getEstimatedMzy());
7103         assertNull(calibrator.getEstimatedCovariance());
7104         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7105         assertNull(calibrator.getGroundTruthGravityNorm());
7106         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7107         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7108         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7109 
7110         // Force IllegalArgumentException
7111         final var m1 = new Matrix(1, 1);
7112         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7113                 measurements, true, m1));
7114         final var m2 = new Matrix(1, 3);
7115         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7116                 measurements, true, m2));
7117     }
7118 
7119     @Test
7120     void testConstructor70() throws WrongSizeException {
7121         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7122 
7123         final var ba = generateBa();
7124         final var biasX = ba.getElementAtIndex(0);
7125         final var biasY = ba.getElementAtIndex(1);
7126         final var biasZ = ba.getElementAtIndex(2);
7127 
7128         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
7129                 ba, this);
7130 
7131         // check default values
7132         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7133         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7134         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7135         final var bx1 = calibrator.getBiasXAsAcceleration();
7136         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7137         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7138         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7139         calibrator.getBiasXAsAcceleration(bx2);
7140         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7141         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7142         final var by1 = calibrator.getBiasYAsAcceleration();
7143         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7144         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7145         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7146         calibrator.getBiasYAsAcceleration(by2);
7147         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7148         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7149         final var bz1 = calibrator.getBiasZAsAcceleration();
7150         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7151         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7152         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7153         calibrator.getBiasZAsAcceleration(bz2);
7154         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7155         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7156         final var biasTriad1 = calibrator.getBiasAsTriad();
7157         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7158         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7159         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7160         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7161         final var biasTriad2 = new AccelerationTriad();
7162         calibrator.getBiasAsTriad(biasTriad2);
7163         assertEquals(biasTriad1, biasTriad2);
7164         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
7165         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
7166         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
7167         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
7168         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
7169         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
7170         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
7171         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
7172         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
7173         final var bias1 = calibrator.getBias();
7174         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7175         final var bias2 = new double[3];
7176         calibrator.getBias(bias2);
7177         assertArrayEquals(bias1, bias2, 0.0);
7178         final var b1 = calibrator.getBiasAsMatrix();
7179         assertEquals(b1, ba);
7180         final var b2 = new Matrix(3, 1);
7181         calibrator.getBiasAsMatrix(b2);
7182         assertEquals(b1, b2);
7183         final var ma1 = calibrator.getInitialMa();
7184         assertEquals(new Matrix(3, 3), ma1);
7185         final var ma2 = new Matrix(3, 3);
7186         calibrator.getInitialMa(ma2);
7187         assertEquals(ma1, ma2);
7188         assertSame(measurements, calibrator.getMeasurements());
7189         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7190                 calibrator.getMeasurementType());
7191         assertFalse(calibrator.isOrderedMeasurementsRequired());
7192         assertFalse(calibrator.isQualityScoresRequired());
7193         assertTrue(calibrator.isCommonAxisUsed());
7194         assertSame(this, calibrator.getListener());
7195         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7196         assertFalse(calibrator.isReady());
7197         assertFalse(calibrator.isRunning());
7198         assertNull(calibrator.getEstimatedMa());
7199         assertNull(calibrator.getEstimatedSx());
7200         assertNull(calibrator.getEstimatedSy());
7201         assertNull(calibrator.getEstimatedSz());
7202         assertNull(calibrator.getEstimatedMxy());
7203         assertNull(calibrator.getEstimatedMxz());
7204         assertNull(calibrator.getEstimatedMyx());
7205         assertNull(calibrator.getEstimatedMyz());
7206         assertNull(calibrator.getEstimatedMzx());
7207         assertNull(calibrator.getEstimatedMzy());
7208         assertNull(calibrator.getEstimatedCovariance());
7209         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7210         assertNull(calibrator.getGroundTruthGravityNorm());
7211         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7212         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7213         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7214 
7215         // Force IllegalArgumentException
7216         final var m1 = new Matrix(1, 1);
7217         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7218                 measurements, true, m1, this));
7219         final var m2 = new Matrix(1, 3);
7220         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7221                 measurements, true, m2, this));
7222     }
7223 
7224     @Test
7225     void testConstructor71() throws WrongSizeException {
7226         final var ba = generateBa();
7227         final var biasX = ba.getElementAtIndex(0);
7228         final var biasY = ba.getElementAtIndex(1);
7229         final var biasZ = ba.getElementAtIndex(2);
7230 
7231         final var ma = generateMaCommonAxis();
7232         final var sx = ma.getElementAt(0, 0);
7233         final var sy = ma.getElementAt(1, 1);
7234         final var sz = ma.getElementAt(2, 2);
7235         final var mxy = ma.getElementAt(0, 1);
7236         final var mxz = ma.getElementAt(0, 2);
7237         final var myx = ma.getElementAt(1, 0);
7238         final var myz = ma.getElementAt(1, 2);
7239         final var mzx = ma.getElementAt(2, 0);
7240         final var mzy = ma.getElementAt(2, 1);
7241 
7242         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba, ma);
7243 
7244         // check default values
7245         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7246         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7247         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7248         final var bx1 = calibrator.getBiasXAsAcceleration();
7249         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7250         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7251         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7252         calibrator.getBiasXAsAcceleration(bx2);
7253         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7254         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7255         final var by1 = calibrator.getBiasYAsAcceleration();
7256         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7257         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7258         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7259         calibrator.getBiasYAsAcceleration(by2);
7260         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7261         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7262         final var bz1 = calibrator.getBiasZAsAcceleration();
7263         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7264         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7265         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7266         calibrator.getBiasZAsAcceleration(bz2);
7267         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7268         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7269         final var biasTriad1 = calibrator.getBiasAsTriad();
7270         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7271         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7272         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7273         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7274         final var biasTriad2 = new AccelerationTriad();
7275         calibrator.getBiasAsTriad(biasTriad2);
7276         assertEquals(biasTriad1, biasTriad2);
7277         assertEquals(sx, calibrator.getInitialSx(), 0.0);
7278         assertEquals(sy, calibrator.getInitialSy(), 0.0);
7279         assertEquals(sz, calibrator.getInitialSz(), 0.0);
7280         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7281         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7282         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7283         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7284         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7285         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7286         final var bias1 = calibrator.getBias();
7287         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7288         final var bias2 = new double[3];
7289         calibrator.getBias(bias2);
7290         assertArrayEquals(bias1, bias2, 0.0);
7291         final var b1 = calibrator.getBiasAsMatrix();
7292         assertEquals(b1, ba);
7293         final var b2 = new Matrix(3, 1);
7294         calibrator.getBiasAsMatrix(b2);
7295         assertEquals(b1, b2);
7296         final var ma1 = new Matrix(3, 3);
7297         ma1.setSubmatrix(0, 0, 2, 2,
7298                 new double[]{sx, myx, mzx,
7299                         mxy, sy, mzy,
7300                         mxz, myz, sz});
7301         assertEquals(calibrator.getInitialMa(), ma1);
7302         final var ma2 = new Matrix(3, 3);
7303         calibrator.getInitialMa(ma2);
7304         assertEquals(ma1, ma2);
7305         assertNull(calibrator.getMeasurements());
7306         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7307                 calibrator.getMeasurementType());
7308         assertFalse(calibrator.isOrderedMeasurementsRequired());
7309         assertFalse(calibrator.isQualityScoresRequired());
7310         assertFalse(calibrator.isCommonAxisUsed());
7311         assertNull(calibrator.getListener());
7312         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7313         assertFalse(calibrator.isReady());
7314         assertFalse(calibrator.isRunning());
7315         assertNull(calibrator.getEstimatedMa());
7316         assertNull(calibrator.getEstimatedSx());
7317         assertNull(calibrator.getEstimatedSy());
7318         assertNull(calibrator.getEstimatedSz());
7319         assertNull(calibrator.getEstimatedMxy());
7320         assertNull(calibrator.getEstimatedMxz());
7321         assertNull(calibrator.getEstimatedMyx());
7322         assertNull(calibrator.getEstimatedMyz());
7323         assertNull(calibrator.getEstimatedMzx());
7324         assertNull(calibrator.getEstimatedMzy());
7325         assertNull(calibrator.getEstimatedCovariance());
7326         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7327         assertNull(calibrator.getGroundTruthGravityNorm());
7328         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7329         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7330         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7331 
7332         // Force IllegalArgumentException
7333         final var m1 = new Matrix(1, 1);
7334         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m1, ma));
7335         final var m2 = new Matrix(1, 3);
7336         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(m2, ma));
7337         final var m3 = new Matrix(1, 3);
7338         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba, m3));
7339         final var m4 = new Matrix(3, 1);
7340         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba, m4));
7341     }
7342 
7343     @Test
7344     void testConstructor72() throws WrongSizeException {
7345         final var ba = generateBa();
7346         final var biasX = ba.getElementAtIndex(0);
7347         final var biasY = ba.getElementAtIndex(1);
7348         final var biasZ = ba.getElementAtIndex(2);
7349 
7350         final var ma = generateMaCommonAxis();
7351         final var sx = ma.getElementAt(0, 0);
7352         final var sy = ma.getElementAt(1, 1);
7353         final var sz = ma.getElementAt(2, 2);
7354         final var mxy = ma.getElementAt(0, 1);
7355         final var mxz = ma.getElementAt(0, 2);
7356         final var myx = ma.getElementAt(1, 0);
7357         final var myz = ma.getElementAt(1, 2);
7358         final var mzx = ma.getElementAt(2, 0);
7359         final var mzy = ma.getElementAt(2, 1);
7360 
7361         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(ba, ma, this);
7362 
7363         // check default values
7364         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7365         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7366         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7367         final var bx1 = calibrator.getBiasXAsAcceleration();
7368         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7369         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7370         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7371         calibrator.getBiasXAsAcceleration(bx2);
7372         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7373         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7374         final var by1 = calibrator.getBiasYAsAcceleration();
7375         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7376         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7377         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7378         calibrator.getBiasYAsAcceleration(by2);
7379         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7380         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7381         final var bz1 = calibrator.getBiasZAsAcceleration();
7382         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7383         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7384         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7385         calibrator.getBiasZAsAcceleration(bz2);
7386         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7387         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7388         final var biasTriad1 = calibrator.getBiasAsTriad();
7389         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7390         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7391         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7392         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7393         final var biasTriad2 = new AccelerationTriad();
7394         calibrator.getBiasAsTriad(biasTriad2);
7395         assertEquals(biasTriad1, biasTriad2);
7396         assertEquals(sx, calibrator.getInitialSx(), 0.0);
7397         assertEquals(sy, calibrator.getInitialSy(), 0.0);
7398         assertEquals(sz, calibrator.getInitialSz(), 0.0);
7399         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7400         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7401         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7402         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7403         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7404         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7405         final var bias1 = calibrator.getBias();
7406         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7407         final var bias2 = new double[3];
7408         calibrator.getBias(bias2);
7409         assertArrayEquals(bias1, bias2, 0.0);
7410         final var b1 = calibrator.getBiasAsMatrix();
7411         assertEquals(b1, ba);
7412         final var b2 = new Matrix(3, 1);
7413         calibrator.getBiasAsMatrix(b2);
7414         assertEquals(b1, b2);
7415         final var ma1 = new Matrix(3, 3);
7416         ma1.setSubmatrix(0, 0, 2, 2,
7417                 new double[]{sx, myx, mzx,
7418                         mxy, sy, mzy,
7419                         mxz, myz, sz});
7420         assertEquals(ma1, calibrator.getInitialMa());
7421         final var ma2 = new Matrix(3, 3);
7422         calibrator.getInitialMa(ma2);
7423         assertEquals(ma1, ma2);
7424         assertNull(calibrator.getMeasurements());
7425         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7426                 calibrator.getMeasurementType());
7427         assertFalse(calibrator.isOrderedMeasurementsRequired());
7428         assertFalse(calibrator.isQualityScoresRequired());
7429         assertFalse(calibrator.isCommonAxisUsed());
7430         assertSame(this, calibrator.getListener());
7431         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7432         assertFalse(calibrator.isReady());
7433         assertFalse(calibrator.isRunning());
7434         assertNull(calibrator.getEstimatedMa());
7435         assertNull(calibrator.getEstimatedSx());
7436         assertNull(calibrator.getEstimatedSy());
7437         assertNull(calibrator.getEstimatedSz());
7438         assertNull(calibrator.getEstimatedMxy());
7439         assertNull(calibrator.getEstimatedMxz());
7440         assertNull(calibrator.getEstimatedMyx());
7441         assertNull(calibrator.getEstimatedMyz());
7442         assertNull(calibrator.getEstimatedMzx());
7443         assertNull(calibrator.getEstimatedMzy());
7444         assertNull(calibrator.getEstimatedCovariance());
7445         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7446         assertNull(calibrator.getGroundTruthGravityNorm());
7447         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7448         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7449         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7450 
7451         // Force IllegalArgumentException
7452         final var m1 = new Matrix(1, 1);
7453         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7454                 m1, ma, this));
7455         final var m2 = new Matrix(1, 3);
7456         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7457                 m2, ma, this));
7458         final var m3 = new Matrix(1, 3);
7459         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7460                 m3, this));
7461         final var m4 = new Matrix(3, 1);
7462         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(ba,
7463                 m4, this));
7464     }
7465 
7466     @Test
7467     void testConstructor73() throws WrongSizeException {
7468         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7469 
7470         final var ba = generateBa();
7471         final var biasX = ba.getElementAtIndex(0);
7472         final var biasY = ba.getElementAtIndex(1);
7473         final var biasZ = ba.getElementAtIndex(2);
7474 
7475         final var ma = generateMaCommonAxis();
7476         final var sx = ma.getElementAt(0, 0);
7477         final var sy = ma.getElementAt(1, 1);
7478         final var sz = ma.getElementAt(2, 2);
7479         final var mxy = ma.getElementAt(0, 1);
7480         final var mxz = ma.getElementAt(0, 2);
7481         final var myx = ma.getElementAt(1, 0);
7482         final var myz = ma.getElementAt(1, 2);
7483         final var mzx = ma.getElementAt(2, 0);
7484         final var mzy = ma.getElementAt(2, 1);
7485 
7486         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba, ma);
7487 
7488         // check default values
7489         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7490         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7491         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7492         final var bx1 = calibrator.getBiasXAsAcceleration();
7493         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7494         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7495         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7496         calibrator.getBiasXAsAcceleration(bx2);
7497         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7498         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7499         final var by1 = calibrator.getBiasYAsAcceleration();
7500         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7501         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7502         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7503         calibrator.getBiasYAsAcceleration(by2);
7504         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7505         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7506         final var bz1 = calibrator.getBiasZAsAcceleration();
7507         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7508         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7509         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7510         calibrator.getBiasZAsAcceleration(bz2);
7511         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7512         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7513         final var biasTriad1 = calibrator.getBiasAsTriad();
7514         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7515         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7516         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7517         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7518         final var biasTriad2 = new AccelerationTriad();
7519         calibrator.getBiasAsTriad(biasTriad2);
7520         assertEquals(biasTriad1, biasTriad2);
7521         assertEquals(sx, calibrator.getInitialSx(), 0.0);
7522         assertEquals(sy, calibrator.getInitialSy(), 0.0);
7523         assertEquals(sz, calibrator.getInitialSz(), 0.0);
7524         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7525         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7526         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7527         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7528         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7529         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7530         final var bias1 = calibrator.getBias();
7531         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7532         final var bias2 = new double[3];
7533         calibrator.getBias(bias2);
7534         assertArrayEquals(bias1, bias2, 0.0);
7535         final var b1 = calibrator.getBiasAsMatrix();
7536         assertEquals(b1, ba);
7537         final var b2 = new Matrix(3, 1);
7538         calibrator.getBiasAsMatrix(b2);
7539         assertEquals(b1, b2);
7540         final var ma1 = new Matrix(3, 3);
7541         ma1.setSubmatrix(0, 0, 2, 2,
7542                 new double[]{sx, myx, mzx,
7543                         mxy, sy, mzy,
7544                         mxz, myz, sz});
7545         assertEquals(ma1, calibrator.getInitialMa());
7546         final var ma2 = new Matrix(3, 3);
7547         calibrator.getInitialMa(ma2);
7548         assertEquals(ma1, ma2);
7549         assertSame(measurements, calibrator.getMeasurements());
7550         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7551                 calibrator.getMeasurementType());
7552         assertFalse(calibrator.isOrderedMeasurementsRequired());
7553         assertFalse(calibrator.isQualityScoresRequired());
7554         assertFalse(calibrator.isCommonAxisUsed());
7555         assertNull(calibrator.getListener());
7556         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7557         assertFalse(calibrator.isReady());
7558         assertFalse(calibrator.isRunning());
7559         assertNull(calibrator.getEstimatedMa());
7560         assertNull(calibrator.getEstimatedSx());
7561         assertNull(calibrator.getEstimatedSy());
7562         assertNull(calibrator.getEstimatedSz());
7563         assertNull(calibrator.getEstimatedMxy());
7564         assertNull(calibrator.getEstimatedMxz());
7565         assertNull(calibrator.getEstimatedMyx());
7566         assertNull(calibrator.getEstimatedMyz());
7567         assertNull(calibrator.getEstimatedMzx());
7568         assertNull(calibrator.getEstimatedMzy());
7569         assertNull(calibrator.getEstimatedCovariance());
7570         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7571         assertNull(calibrator.getGroundTruthGravityNorm());
7572         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7573         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7574         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7575 
7576         // Force IllegalArgumentException
7577         final var m1 = new Matrix(1, 1);
7578         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7579                 measurements, m1, ma));
7580         final var m2 = new Matrix(1, 3);
7581         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7582                 measurements, m2, ma));
7583         final var m3 = new Matrix(1, 3);
7584         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7585                 measurements, ba, m3));
7586         final var m4 = new Matrix(3, 1);
7587         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7588                 measurements, ba, m4));
7589     }
7590 
7591     @Test
7592     void testConstructor74() throws WrongSizeException {
7593         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7594 
7595         final var ba = generateBa();
7596         final var biasX = ba.getElementAtIndex(0);
7597         final var biasY = ba.getElementAtIndex(1);
7598         final var biasZ = ba.getElementAtIndex(2);
7599 
7600         final var ma = generateMaCommonAxis();
7601         final var sx = ma.getElementAt(0, 0);
7602         final var sy = ma.getElementAt(1, 1);
7603         final var sz = ma.getElementAt(2, 2);
7604         final var mxy = ma.getElementAt(0, 1);
7605         final var mxz = ma.getElementAt(0, 2);
7606         final var myx = ma.getElementAt(1, 0);
7607         final var myz = ma.getElementAt(1, 2);
7608         final var mzx = ma.getElementAt(2, 0);
7609         final var mzy = ma.getElementAt(2, 1);
7610 
7611         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, ba, ma, this);
7612 
7613         // check default values
7614         assertEquals(calibrator.getBiasX(), biasX, 0.0);
7615         assertEquals(calibrator.getBiasY(), biasY, 0.0);
7616         assertEquals(calibrator.getBiasZ(), biasZ, 0.0);
7617         final var bx1 = calibrator.getBiasXAsAcceleration();
7618         assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7619         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7620         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7621         calibrator.getBiasXAsAcceleration(bx2);
7622         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7623         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7624         final var by1 = calibrator.getBiasYAsAcceleration();
7625         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7626         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7627         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7628         calibrator.getBiasYAsAcceleration(by2);
7629         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7630         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7631         final var bz1 = calibrator.getBiasZAsAcceleration();
7632         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7633         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7634         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7635         calibrator.getBiasZAsAcceleration(bz2);
7636         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7637         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7638         final var biasTriad1 = calibrator.getBiasAsTriad();
7639         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7640         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7641         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7642         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7643         final var biasTriad2 = new AccelerationTriad();
7644         calibrator.getBiasAsTriad(biasTriad2);
7645         assertEquals(biasTriad1, biasTriad2);
7646         assertEquals(sx, calibrator.getInitialSx(), 0.0);
7647         assertEquals(sy, calibrator.getInitialSy(), 0.0);
7648         assertEquals(sz, calibrator.getInitialSz(), 0.0);
7649         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7650         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7651         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7652         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7653         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7654         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7655         final var bias1 = calibrator.getBias();
7656         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7657         final var bias2 = new double[3];
7658         calibrator.getBias(bias2);
7659         assertArrayEquals(bias1, bias2, 0.0);
7660         final var b1 = calibrator.getBiasAsMatrix();
7661         assertEquals(b1, ba);
7662         final var b2 = new Matrix(3, 1);
7663         calibrator.getBiasAsMatrix(b2);
7664         assertEquals(b1, b2);
7665         final var ma1 = new Matrix(3, 3);
7666         ma1.setSubmatrix(0, 0, 2, 2,
7667                 new double[]{sx, myx, mzx,
7668                         mxy, sy, mzy,
7669                         mxz, myz, sz});
7670         assertEquals(ma1, calibrator.getInitialMa());
7671         final var ma2 = new Matrix(3, 3);
7672         calibrator.getInitialMa(ma2);
7673         assertEquals(ma1, ma2);
7674         assertSame(measurements, calibrator.getMeasurements());
7675         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7676                 calibrator.getMeasurementType());
7677         assertFalse(calibrator.isOrderedMeasurementsRequired());
7678         assertFalse(calibrator.isQualityScoresRequired());
7679         assertFalse(calibrator.isCommonAxisUsed());
7680         assertSame(this, calibrator.getListener());
7681         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
7682         assertFalse(calibrator.isReady());
7683         assertFalse(calibrator.isRunning());
7684         assertNull(calibrator.getEstimatedMa());
7685         assertNull(calibrator.getEstimatedSx());
7686         assertNull(calibrator.getEstimatedSy());
7687         assertNull(calibrator.getEstimatedSz());
7688         assertNull(calibrator.getEstimatedMxy());
7689         assertNull(calibrator.getEstimatedMxz());
7690         assertNull(calibrator.getEstimatedMyx());
7691         assertNull(calibrator.getEstimatedMyz());
7692         assertNull(calibrator.getEstimatedMzx());
7693         assertNull(calibrator.getEstimatedMzy());
7694         assertNull(calibrator.getEstimatedCovariance());
7695         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7696         assertNull(calibrator.getGroundTruthGravityNorm());
7697         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7698         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7699         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7700 
7701         // Force IllegalArgumentException
7702         final var m1 = new Matrix(1, 1);
7703         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7704                 measurements, m1, ma, this));
7705         final var m2 = new Matrix(1, 3);
7706         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7707                 measurements, m2, ma, this));
7708         final var m3 = new Matrix(1, 3);
7709         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7710                 measurements, ba, m3, this));
7711         final var m4 = new Matrix(3, 1);
7712         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7713                 measurements, ba, m4, this));
7714     }
7715 
7716     @Test
7717     void testConstructor75() throws WrongSizeException {
7718         final var ba = generateBa();
7719         final var biasX = ba.getElementAtIndex(0);
7720         final var biasY = ba.getElementAtIndex(1);
7721         final var biasZ = ba.getElementAtIndex(2);
7722 
7723         final var ma = generateMaCommonAxis();
7724         final var sx = ma.getElementAt(0, 0);
7725         final var sy = ma.getElementAt(1, 1);
7726         final var sz = ma.getElementAt(2, 2);
7727         final var mxy = ma.getElementAt(0, 1);
7728         final var mxz = ma.getElementAt(0, 2);
7729         final var myx = ma.getElementAt(1, 0);
7730         final var myz = ma.getElementAt(1, 2);
7731         final var mzx = ma.getElementAt(2, 0);
7732         final var mzy = ma.getElementAt(2, 1);
7733 
7734         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba, ma);
7735 
7736         // check default values
7737         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7738         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7739         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7740         final var bx1 = calibrator.getBiasXAsAcceleration();
7741         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7742         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7743         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7744         calibrator.getBiasXAsAcceleration(bx2);
7745         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7746         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7747         final var by1 = calibrator.getBiasYAsAcceleration();
7748         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7749         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7750         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7751         calibrator.getBiasYAsAcceleration(by2);
7752         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7753         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7754         final var bz1 = calibrator.getBiasZAsAcceleration();
7755         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7756         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7757         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7758         calibrator.getBiasZAsAcceleration(bz2);
7759         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7760         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7761         final var biasTriad1 = calibrator.getBiasAsTriad();
7762         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7763         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7764         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7765         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7766         final var biasTriad2 = new AccelerationTriad();
7767         calibrator.getBiasAsTriad(biasTriad2);
7768         assertEquals(biasTriad1, biasTriad2);
7769         assertEquals(sx, calibrator.getInitialSx(), 0.0);
7770         assertEquals(sy, calibrator.getInitialSy(), 0.0);
7771         assertEquals(sz, calibrator.getInitialSz(), 0.0);
7772         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7773         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7774         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7775         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7776         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7777         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7778         final var bias1 = calibrator.getBias();
7779         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7780         final var bias2 = new double[3];
7781         calibrator.getBias(bias2);
7782         assertArrayEquals(bias1, bias2, 0.0);
7783         final var b1 = calibrator.getBiasAsMatrix();
7784         assertEquals(b1, ba);
7785         final var b2 = new Matrix(3, 1);
7786         calibrator.getBiasAsMatrix(b2);
7787         assertEquals(b1, b2);
7788         final var ma1 = new Matrix(3, 3);
7789         ma1.setSubmatrix(0, 0, 2, 2,
7790                 new double[]{sx, myx, mzx,
7791                         mxy, sy, mzy,
7792                         mxz, myz, sz});
7793         assertEquals(calibrator.getInitialMa(), ma1);
7794         final var ma2 = new Matrix(3, 3);
7795         calibrator.getInitialMa(ma2);
7796         assertEquals(ma1, ma2);
7797         assertNull(calibrator.getMeasurements());
7798         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7799                 calibrator.getMeasurementType());
7800         assertFalse(calibrator.isOrderedMeasurementsRequired());
7801         assertFalse(calibrator.isQualityScoresRequired());
7802         assertTrue(calibrator.isCommonAxisUsed());
7803         assertNull(calibrator.getListener());
7804         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7805         assertFalse(calibrator.isReady());
7806         assertFalse(calibrator.isRunning());
7807         assertNull(calibrator.getEstimatedMa());
7808         assertNull(calibrator.getEstimatedSx());
7809         assertNull(calibrator.getEstimatedSy());
7810         assertNull(calibrator.getEstimatedSz());
7811         assertNull(calibrator.getEstimatedMxy());
7812         assertNull(calibrator.getEstimatedMxz());
7813         assertNull(calibrator.getEstimatedMyx());
7814         assertNull(calibrator.getEstimatedMyz());
7815         assertNull(calibrator.getEstimatedMzx());
7816         assertNull(calibrator.getEstimatedMzy());
7817         assertNull(calibrator.getEstimatedCovariance());
7818         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7819         assertNull(calibrator.getGroundTruthGravityNorm());
7820         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7821         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7822         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7823 
7824         // Force IllegalArgumentException
7825         final var m1 = new Matrix(1, 1);
7826         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7827                 true, m1, ma));
7828         final var m2 = new Matrix(1, 3);
7829         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7830                 true, m2, ma));
7831         final var m3 = new Matrix(1, 3);
7832         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7833                 true, ba, m3));
7834         final var m4 = new Matrix(3, 1);
7835         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7836                 true, ba, m4));
7837     }
7838 
7839     @Test
7840     void testConstructor76() throws WrongSizeException {
7841         final var ba = generateBa();
7842         final var biasX = ba.getElementAtIndex(0);
7843         final var biasY = ba.getElementAtIndex(1);
7844         final var biasZ = ba.getElementAtIndex(2);
7845 
7846         final var ma = generateMaCommonAxis();
7847         final var sx = ma.getElementAt(0, 0);
7848         final var sy = ma.getElementAt(1, 1);
7849         final var sz = ma.getElementAt(2, 2);
7850         final var mxy = ma.getElementAt(0, 1);
7851         final var mxz = ma.getElementAt(0, 2);
7852         final var myx = ma.getElementAt(1, 0);
7853         final var myz = ma.getElementAt(1, 2);
7854         final var mzx = ma.getElementAt(2, 0);
7855         final var mzy = ma.getElementAt(2, 1);
7856 
7857         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(true, ba, ma,
7858                 this);
7859 
7860         // check default values
7861         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7862         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7863         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7864         final var bx1 = calibrator.getBiasXAsAcceleration();
7865         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7866         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7867         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7868         calibrator.getBiasXAsAcceleration(bx2);
7869         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7870         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7871         final var by1 = calibrator.getBiasYAsAcceleration();
7872         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
7873         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
7874         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7875         calibrator.getBiasYAsAcceleration(by2);
7876         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
7877         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
7878         final var bz1 = calibrator.getBiasZAsAcceleration();
7879         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
7880         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
7881         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7882         calibrator.getBiasZAsAcceleration(bz2);
7883         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
7884         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
7885         final var biasTriad1 = calibrator.getBiasAsTriad();
7886         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
7887         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
7888         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
7889         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
7890         final var biasTriad2 = new AccelerationTriad();
7891         calibrator.getBiasAsTriad(biasTriad2);
7892         assertEquals(biasTriad1, biasTriad2);
7893         assertEquals(sx, calibrator.getInitialSx(), 0.0);
7894         assertEquals(sy, calibrator.getInitialSy(), 0.0);
7895         assertEquals(sz, calibrator.getInitialSz(), 0.0);
7896         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
7897         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
7898         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
7899         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
7900         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
7901         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
7902         final var bias1 = calibrator.getBias();
7903         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7904         final var bias2 = new double[3];
7905         calibrator.getBias(bias2);
7906         assertArrayEquals(bias1, bias2, 0.0);
7907         final var b1 = calibrator.getBiasAsMatrix();
7908         assertEquals(b1, ba);
7909         final var b2 = new Matrix(3, 1);
7910         calibrator.getBiasAsMatrix(b2);
7911         assertEquals(b1, b2);
7912         final var ma1 = new Matrix(3, 3);
7913         ma1.setSubmatrix(0, 0, 2, 2,
7914                 new double[]{sx, myx, mzx,
7915                         mxy, sy, mzy,
7916                         mxz, myz, sz});
7917         assertEquals(ma1, calibrator.getInitialMa());
7918         final var ma2 = new Matrix(3, 3);
7919         calibrator.getInitialMa(ma2);
7920         assertEquals(ma1, ma2);
7921         assertNull(calibrator.getMeasurements());
7922         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
7923                 calibrator.getMeasurementType());
7924         assertFalse(calibrator.isOrderedMeasurementsRequired());
7925         assertFalse(calibrator.isQualityScoresRequired());
7926         assertTrue(calibrator.isCommonAxisUsed());
7927         assertSame(this, calibrator.getListener());
7928         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
7929         assertFalse(calibrator.isReady());
7930         assertFalse(calibrator.isRunning());
7931         assertNull(calibrator.getEstimatedMa());
7932         assertNull(calibrator.getEstimatedSx());
7933         assertNull(calibrator.getEstimatedSy());
7934         assertNull(calibrator.getEstimatedSz());
7935         assertNull(calibrator.getEstimatedMxy());
7936         assertNull(calibrator.getEstimatedMxz());
7937         assertNull(calibrator.getEstimatedMyx());
7938         assertNull(calibrator.getEstimatedMyz());
7939         assertNull(calibrator.getEstimatedMzx());
7940         assertNull(calibrator.getEstimatedMzy());
7941         assertNull(calibrator.getEstimatedCovariance());
7942         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
7943         assertNull(calibrator.getGroundTruthGravityNorm());
7944         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7945         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7946         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
7947 
7948         // Force IllegalArgumentException
7949         final var m1 = new Matrix(1, 1);
7950         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7951                 true, m1, ma, this));
7952         final var m2 = new Matrix(1, 3);
7953         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7954                 true, m2, ma, this));
7955         final var m3 = new Matrix(1, 3);
7956         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7957                 true, ba, m3, this));
7958         final var m4 = new Matrix(3, 1);
7959         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
7960                 true, ba, m4, this));
7961     }
7962 
7963     @Test
7964     void testConstructor77() throws WrongSizeException {
7965         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
7966 
7967         final var ba = generateBa();
7968         final var biasX = ba.getElementAtIndex(0);
7969         final var biasY = ba.getElementAtIndex(1);
7970         final var biasZ = ba.getElementAtIndex(2);
7971 
7972         final var ma = generateMaCommonAxis();
7973         final var sx = ma.getElementAt(0, 0);
7974         final var sy = ma.getElementAt(1, 1);
7975         final var sz = ma.getElementAt(2, 2);
7976         final var mxy = ma.getElementAt(0, 1);
7977         final var mxz = ma.getElementAt(0, 2);
7978         final var myx = ma.getElementAt(1, 0);
7979         final var myz = ma.getElementAt(1, 2);
7980         final var mzx = ma.getElementAt(2, 0);
7981         final var mzy = ma.getElementAt(2, 1);
7982 
7983         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
7984                 ba, ma);
7985 
7986         // check default values
7987         assertEquals(biasX, calibrator.getBiasX(), 0.0);
7988         assertEquals(biasY, calibrator.getBiasY(), 0.0);
7989         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
7990         final var bx1 = calibrator.getBiasXAsAcceleration();
7991         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
7992         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
7993         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
7994         calibrator.getBiasXAsAcceleration(bx2);
7995         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
7996         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
7997         final var by1 = calibrator.getBiasYAsAcceleration();
7998         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7999         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8000         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8001         calibrator.getBiasYAsAcceleration(by2);
8002         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
8003         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8004         final var bz1 = calibrator.getBiasZAsAcceleration();
8005         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
8006         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8007         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8008         calibrator.getBiasZAsAcceleration(bz2);
8009         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
8010         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8011         final var biasTriad1 = calibrator.getBiasAsTriad();
8012         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
8013         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
8014         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
8015         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8016         final var biasTriad2 = new AccelerationTriad();
8017         calibrator.getBiasAsTriad(biasTriad2);
8018         assertEquals(biasTriad1, biasTriad2);
8019         assertEquals(sx, calibrator.getInitialSx(), 0.0);
8020         assertEquals(sy, calibrator.getInitialSy(), 0.0);
8021         assertEquals(sz, calibrator.getInitialSz(), 0.0);
8022         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
8023         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
8024         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
8025         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
8026         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
8027         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
8028         final var bias1 = calibrator.getBias();
8029         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8030         final var bias2 = new double[3];
8031         calibrator.getBias(bias2);
8032         assertArrayEquals(bias1, bias2, 0.0);
8033         final var b1 = calibrator.getBiasAsMatrix();
8034         assertEquals(b1, ba);
8035         final var b2 = new Matrix(3, 1);
8036         calibrator.getBiasAsMatrix(b2);
8037         assertEquals(b1, b2);
8038         final var ma1 = new Matrix(3, 3);
8039         ma1.setSubmatrix(0, 0, 2, 2,
8040                 new double[]{sx, myx, mzx,
8041                         mxy, sy, mzy,
8042                         mxz, myz, sz});
8043         assertEquals(ma1, calibrator.getInitialMa());
8044         final var ma2 = new Matrix(3, 3);
8045         calibrator.getInitialMa(ma2);
8046         assertEquals(ma1, ma2);
8047         assertSame(measurements, calibrator.getMeasurements());
8048         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8049                 calibrator.getMeasurementType());
8050         assertFalse(calibrator.isOrderedMeasurementsRequired());
8051         assertFalse(calibrator.isQualityScoresRequired());
8052         assertTrue(calibrator.isCommonAxisUsed());
8053         assertNull(calibrator.getListener());
8054         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8055         assertFalse(calibrator.isReady());
8056         assertFalse(calibrator.isRunning());
8057         assertNull(calibrator.getEstimatedMa());
8058         assertNull(calibrator.getEstimatedSx());
8059         assertNull(calibrator.getEstimatedSy());
8060         assertNull(calibrator.getEstimatedSz());
8061         assertNull(calibrator.getEstimatedMxy());
8062         assertNull(calibrator.getEstimatedMxz());
8063         assertNull(calibrator.getEstimatedMyx());
8064         assertNull(calibrator.getEstimatedMyz());
8065         assertNull(calibrator.getEstimatedMzx());
8066         assertNull(calibrator.getEstimatedMzy());
8067         assertNull(calibrator.getEstimatedCovariance());
8068         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8069         assertNull(calibrator.getGroundTruthGravityNorm());
8070         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8071         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8072         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8073 
8074         // Force IllegalArgumentException
8075         final var m1 = new Matrix(1, 1);
8076         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8077                 measurements, true, m1, ma));
8078         final var m2 = new Matrix(1, 3);
8079         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8080                 measurements, true, m2, ma));
8081         final var m3 = new Matrix(1, 3);
8082         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8083                 measurements, true, ba, m3));
8084         final var m4 = new Matrix(3, 1);
8085         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8086                 measurements, true, ba, m4));
8087     }
8088 
8089     @Test
8090     void testConstructor78() throws WrongSizeException {
8091         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8092 
8093         final var ba = generateBa();
8094         final var biasX = ba.getElementAtIndex(0);
8095         final var biasY = ba.getElementAtIndex(1);
8096         final var biasZ = ba.getElementAtIndex(2);
8097 
8098         final var ma = generateMaCommonAxis();
8099         final var sx = ma.getElementAt(0, 0);
8100         final var sy = ma.getElementAt(1, 1);
8101         final var sz = ma.getElementAt(2, 2);
8102         final var mxy = ma.getElementAt(0, 1);
8103         final var mxz = ma.getElementAt(0, 2);
8104         final var myx = ma.getElementAt(1, 0);
8105         final var myz = ma.getElementAt(1, 2);
8106         final var mzx = ma.getElementAt(2, 0);
8107         final var mzy = ma.getElementAt(2, 1);
8108 
8109         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(measurements, true,
8110                 ba, ma, this);
8111 
8112         // check default values
8113         assertEquals(biasX, calibrator.getBiasX(), 0.0);
8114         assertEquals(biasY, calibrator.getBiasY(), 0.0);
8115         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
8116         final var bx1 = calibrator.getBiasXAsAcceleration();
8117         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
8118         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8119         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8120         calibrator.getBiasXAsAcceleration(bx2);
8121         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
8122         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8123         final var by1 = calibrator.getBiasYAsAcceleration();
8124         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
8125         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8126         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8127         calibrator.getBiasYAsAcceleration(by2);
8128         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
8129         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8130         final var bz1 = calibrator.getBiasZAsAcceleration();
8131         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
8132         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8133         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8134         calibrator.getBiasZAsAcceleration(bz2);
8135         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
8136         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8137         final var biasTriad1 = calibrator.getBiasAsTriad();
8138         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
8139         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
8140         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
8141         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8142         final var biasTriad2 = new AccelerationTriad();
8143         calibrator.getBiasAsTriad(biasTriad2);
8144         assertEquals(biasTriad1, biasTriad2);
8145         assertEquals(sx, calibrator.getInitialSx(), 0.0);
8146         assertEquals(sy, calibrator.getInitialSy(), 0.0);
8147         assertEquals(sz, calibrator.getInitialSz(), 0.0);
8148         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
8149         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
8150         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
8151         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
8152         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
8153         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
8154         final var bias1 = calibrator.getBias();
8155         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8156         final var bias2 = new double[3];
8157         calibrator.getBias(bias2);
8158         assertArrayEquals(bias1, bias2, 0.0);
8159         final var b1 = calibrator.getBiasAsMatrix();
8160         assertEquals(b1, ba);
8161         final var b2 = new Matrix(3, 1);
8162         calibrator.getBiasAsMatrix(b2);
8163         assertEquals(b1, b2);
8164         final var ma1 = new Matrix(3, 3);
8165         ma1.setSubmatrix(0, 0, 2, 2,
8166                 new double[]{sx, myx, mzx,
8167                         mxy, sy, mzy,
8168                         mxz, myz, sz});
8169         assertEquals(ma1, calibrator.getInitialMa());
8170         final var ma2 = new Matrix(3, 3);
8171         calibrator.getInitialMa(ma2);
8172         assertEquals(ma1, ma2);
8173         assertSame(measurements, calibrator.getMeasurements());
8174         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8175                 calibrator.getMeasurementType());
8176         assertFalse(calibrator.isOrderedMeasurementsRequired());
8177         assertFalse(calibrator.isQualityScoresRequired());
8178         assertTrue(calibrator.isCommonAxisUsed());
8179         assertSame(this, calibrator.getListener());
8180         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8181         assertFalse(calibrator.isReady());
8182         assertFalse(calibrator.isRunning());
8183         assertNull(calibrator.getEstimatedMa());
8184         assertNull(calibrator.getEstimatedSx());
8185         assertNull(calibrator.getEstimatedSy());
8186         assertNull(calibrator.getEstimatedSz());
8187         assertNull(calibrator.getEstimatedMxy());
8188         assertNull(calibrator.getEstimatedMxz());
8189         assertNull(calibrator.getEstimatedMyx());
8190         assertNull(calibrator.getEstimatedMyz());
8191         assertNull(calibrator.getEstimatedMzx());
8192         assertNull(calibrator.getEstimatedMzy());
8193         assertNull(calibrator.getEstimatedCovariance());
8194         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8195         assertNull(calibrator.getGroundTruthGravityNorm());
8196         assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8197         assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8198         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8199 
8200         // Force IllegalArgumentException
8201         final var m1 = new Matrix(1, 1);
8202         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8203                 measurements, true, m1, ma, this));
8204         final var m2 = new Matrix(1, 3);
8205         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8206                 measurements, true, m2, ma, this));
8207         final var m3 = new Matrix(1, 3);
8208         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8209                 measurements, true, ba, m3, this));
8210         final var m4 = new Matrix(3, 1);
8211         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8212                 measurements, true, ba, m4, this));
8213     }
8214 
8215     @Test
8216     void testConstructor79() throws WrongSizeException {
8217         final var randomizer = new UniformRandomizer();
8218         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8219         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8220         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8221         final var nedPosition = new NEDPosition(latitude, longitude, height);
8222         final var nedVelocity = new NEDVelocity();
8223         final var ecefPosition = new ECEFPosition();
8224         final var ecefVelocity = new ECEFVelocity();
8225         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8226         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8227                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8228         final var gravityNorm = gravity.getNorm();
8229 
8230         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
8231 
8232         // check default values
8233         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8234         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8235         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8236         final var bx1 = calibrator.getBiasXAsAcceleration();
8237         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8238         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8239         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8240         calibrator.getBiasXAsAcceleration(bx2);
8241         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8242         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8243         final var by1 = calibrator.getBiasYAsAcceleration();
8244         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8245         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8246         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8247         calibrator.getBiasYAsAcceleration(by2);
8248         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8249         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8250         final var bz1 = calibrator.getBiasZAsAcceleration();
8251         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8252         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8253         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8254         calibrator.getBiasZAsAcceleration(bz2);
8255         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8256         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8257         final var biasTriad1 = calibrator.getBiasAsTriad();
8258         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8259         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8260         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8261         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8262         final var biasTriad2 = new AccelerationTriad();
8263         calibrator.getBiasAsTriad(biasTriad2);
8264         assertEquals(biasTriad1, biasTriad2);
8265         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8266         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8267         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8268         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8269         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8270         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8271         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8272         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8273         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8274         final var bias1 = calibrator.getBias();
8275         assertArrayEquals(new double[3], bias1, 0.0);
8276         final var bias2 = new double[3];
8277         calibrator.getBias(bias2);
8278         assertArrayEquals(bias1, bias2, 0.0);
8279         final var b1 = calibrator.getBiasAsMatrix();
8280         assertEquals(new Matrix(3, 1), b1);
8281         final var b2 = new Matrix(3, 1);
8282         calibrator.getBiasAsMatrix(b2);
8283         assertEquals(b1, b2);
8284         final var ma1 = calibrator.getInitialMa();
8285         assertEquals(new Matrix(3, 3), ma1);
8286         final var ma2 = new Matrix(3, 3);
8287         calibrator.getInitialMa(ma2);
8288         assertEquals(ma1, ma2);
8289         assertNull(calibrator.getMeasurements());
8290         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8291                 calibrator.getMeasurementType());
8292         assertFalse(calibrator.isOrderedMeasurementsRequired());
8293         assertFalse(calibrator.isQualityScoresRequired());
8294         assertFalse(calibrator.isCommonAxisUsed());
8295         assertNull(calibrator.getListener());
8296         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8297         assertFalse(calibrator.isReady());
8298         assertFalse(calibrator.isRunning());
8299         assertNull(calibrator.getEstimatedMa());
8300         assertNull(calibrator.getEstimatedSx());
8301         assertNull(calibrator.getEstimatedSy());
8302         assertNull(calibrator.getEstimatedSz());
8303         assertNull(calibrator.getEstimatedMxy());
8304         assertNull(calibrator.getEstimatedMxz());
8305         assertNull(calibrator.getEstimatedMyx());
8306         assertNull(calibrator.getEstimatedMyz());
8307         assertNull(calibrator.getEstimatedMzx());
8308         assertNull(calibrator.getEstimatedMzy());
8309         assertNull(calibrator.getEstimatedCovariance());
8310         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8311         assertNotNull(calibrator.getGroundTruthGravityNorm());
8312         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8313         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8314         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8315                 ABSOLUTE_ERROR));
8316         final var g = new Acceleration(0.0, AccelerationUnit.G);
8317         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8318         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8319         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8320 
8321         // Force IllegalArgumentException
8322         assertThrows(IllegalArgumentException.class,
8323                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm));
8324     }
8325 
8326     @Test
8327     void testConstructor80() throws WrongSizeException {
8328         final var randomizer = new UniformRandomizer();
8329         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8330         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8331         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8332         final var nedPosition = new NEDPosition(latitude, longitude, height);
8333         final var nedVelocity = new NEDVelocity();
8334         final var ecefPosition = new ECEFPosition();
8335         final var ecefVelocity = new ECEFVelocity();
8336         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8337         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8338                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8339         final var gravityNorm = gravity.getNorm();
8340 
8341         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, this);
8342 
8343         // check default values
8344         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8345         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8346         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8347         final var bx1 = calibrator.getBiasXAsAcceleration();
8348         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8349         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8350         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8351         calibrator.getBiasXAsAcceleration(bx2);
8352         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8353         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8354         final var by1 = calibrator.getBiasYAsAcceleration();
8355         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8356         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8357         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8358         calibrator.getBiasYAsAcceleration(by2);
8359         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8360         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8361         final var bz1 = calibrator.getBiasZAsAcceleration();
8362         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8363         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8364         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8365         calibrator.getBiasZAsAcceleration(bz2);
8366         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8367         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8368         final var biasTriad1 = calibrator.getBiasAsTriad();
8369         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8370         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8371         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8372         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8373         final var biasTriad2 = new AccelerationTriad();
8374         calibrator.getBiasAsTriad(biasTriad2);
8375         assertEquals(biasTriad1, biasTriad2);
8376         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8377         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8378         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8379         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8380         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8381         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8382         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8383         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8384         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8385         final var bias1 = calibrator.getBias();
8386         assertArrayEquals(new double[3], bias1, 0.0);
8387         final var bias2 = new double[3];
8388         calibrator.getBias(bias2);
8389         assertArrayEquals(bias1, bias2, 0.0);
8390         final var b1 = calibrator.getBiasAsMatrix();
8391         assertEquals(new Matrix(3, 1), b1);
8392         final var b2 = new Matrix(3, 1);
8393         calibrator.getBiasAsMatrix(b2);
8394         assertEquals(b1, b2);
8395         final var ma1 = calibrator.getInitialMa();
8396         assertEquals(new Matrix(3, 3), ma1);
8397         final var ma2 = new Matrix(3, 3);
8398         calibrator.getInitialMa(ma2);
8399         assertEquals(ma1, ma2);
8400         assertNull(calibrator.getMeasurements());
8401         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8402                 calibrator.getMeasurementType());
8403         assertFalse(calibrator.isOrderedMeasurementsRequired());
8404         assertFalse(calibrator.isQualityScoresRequired());
8405         assertFalse(calibrator.isCommonAxisUsed());
8406         assertSame(this, calibrator.getListener());
8407         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8408         assertFalse(calibrator.isReady());
8409         assertFalse(calibrator.isRunning());
8410         assertNull(calibrator.getEstimatedMa());
8411         assertNull(calibrator.getEstimatedSx());
8412         assertNull(calibrator.getEstimatedSy());
8413         assertNull(calibrator.getEstimatedSz());
8414         assertNull(calibrator.getEstimatedMxy());
8415         assertNull(calibrator.getEstimatedMxz());
8416         assertNull(calibrator.getEstimatedMyx());
8417         assertNull(calibrator.getEstimatedMyz());
8418         assertNull(calibrator.getEstimatedMzx());
8419         assertNull(calibrator.getEstimatedMzy());
8420         assertNull(calibrator.getEstimatedCovariance());
8421         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8422         assertNotNull(calibrator.getGroundTruthGravityNorm());
8423         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8424         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8425         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8426                 ABSOLUTE_ERROR));
8427         final var g = new Acceleration(0.0, AccelerationUnit.G);
8428         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8429         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8430         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8431 
8432         // Force IllegalArgumentException
8433         assertThrows(IllegalArgumentException.class,
8434                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, this));
8435     }
8436 
8437     @Test
8438     void testConstructor81() throws WrongSizeException {
8439         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8440 
8441         final var randomizer = new UniformRandomizer();
8442         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8443         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8444         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8445         final var nedPosition = new NEDPosition(latitude, longitude, height);
8446         final var nedVelocity = new NEDVelocity();
8447         final var ecefPosition = new ECEFPosition();
8448         final var ecefVelocity = new ECEFVelocity();
8449         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8450         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8451                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8452         final var gravityNorm = gravity.getNorm();
8453 
8454         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements);
8455 
8456         // check default values
8457         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8458         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8459         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8460         final var bx1 = calibrator.getBiasXAsAcceleration();
8461         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8462         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8463         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8464         calibrator.getBiasXAsAcceleration(bx2);
8465         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8466         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8467         final var by1 = calibrator.getBiasYAsAcceleration();
8468         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8469         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8470         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8471         calibrator.getBiasYAsAcceleration(by2);
8472         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8473         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8474         final var bz1 = calibrator.getBiasZAsAcceleration();
8475         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8476         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8477         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8478         calibrator.getBiasZAsAcceleration(bz2);
8479         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8480         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8481         final var biasTriad1 = calibrator.getBiasAsTriad();
8482         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8483         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8484         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8485         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8486         final var biasTriad2 = new AccelerationTriad();
8487         calibrator.getBiasAsTriad(biasTriad2);
8488         assertEquals(biasTriad1, biasTriad2);
8489         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8490         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8491         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8492         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8493         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8494         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8495         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8496         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8497         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8498         final var bias1 = calibrator.getBias();
8499         assertArrayEquals(new double[3], bias1, 0.0);
8500         final var bias2 = new double[3];
8501         calibrator.getBias(bias2);
8502         assertArrayEquals(bias2, bias1, 0.0);
8503         final var b1 = calibrator.getBiasAsMatrix();
8504         assertEquals(new Matrix(3, 1), b1);
8505         final var b2 = new Matrix(3, 1);
8506         calibrator.getBiasAsMatrix(b2);
8507         assertEquals(b1, b2);
8508         final var ma1 = calibrator.getInitialMa();
8509         assertEquals(new Matrix(3, 3), ma1);
8510         final var ma2 = new Matrix(3, 3);
8511         calibrator.getInitialMa(ma2);
8512         assertEquals(ma1, ma2);
8513         assertSame(measurements, calibrator.getMeasurements());
8514         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8515                 calibrator.getMeasurementType());
8516         assertFalse(calibrator.isOrderedMeasurementsRequired());
8517         assertFalse(calibrator.isQualityScoresRequired());
8518         assertFalse(calibrator.isCommonAxisUsed());
8519         assertNull(calibrator.getListener());
8520         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8521         assertFalse(calibrator.isReady());
8522         assertFalse(calibrator.isRunning());
8523         assertNull(calibrator.getEstimatedMa());
8524         assertNull(calibrator.getEstimatedSx());
8525         assertNull(calibrator.getEstimatedSy());
8526         assertNull(calibrator.getEstimatedSz());
8527         assertNull(calibrator.getEstimatedMxy());
8528         assertNull(calibrator.getEstimatedMxz());
8529         assertNull(calibrator.getEstimatedMyx());
8530         assertNull(calibrator.getEstimatedMyz());
8531         assertNull(calibrator.getEstimatedMzx());
8532         assertNull(calibrator.getEstimatedMzy());
8533         assertNull(calibrator.getEstimatedCovariance());
8534         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8535         assertNotNull(calibrator.getGroundTruthGravityNorm());
8536         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8537         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8538         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8539                 ABSOLUTE_ERROR));
8540         final var g = new Acceleration(0.0, AccelerationUnit.G);
8541         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8542         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8543         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8544 
8545         // Force IllegalArgumentException
8546         assertThrows(IllegalArgumentException.class,
8547                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, measurements));
8548     }
8549 
8550     @Test
8551     void testConstructor82() throws WrongSizeException {
8552         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8553 
8554         final var randomizer = new UniformRandomizer();
8555         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8556         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8557         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8558         final var nedPosition = new NEDPosition(latitude, longitude, height);
8559         final var nedVelocity = new NEDVelocity();
8560         final var ecefPosition = new ECEFPosition();
8561         final var ecefVelocity = new ECEFVelocity();
8562         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8563         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8564                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8565         final var gravityNorm = gravity.getNorm();
8566 
8567         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
8568                 this);
8569 
8570         // check default values
8571         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8572         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8573         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8574         final var bx1 = calibrator.getBiasXAsAcceleration();
8575         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8576         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8577         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8578         calibrator.getBiasXAsAcceleration(bx2);
8579         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8580         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8581         final var by1 = calibrator.getBiasYAsAcceleration();
8582         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8583         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8584         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8585         calibrator.getBiasYAsAcceleration(by2);
8586         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8587         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8588         final var bz1 = calibrator.getBiasZAsAcceleration();
8589         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8590         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8591         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8592         calibrator.getBiasZAsAcceleration(bz2);
8593         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8594         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8595         final var biasTriad1 = calibrator.getBiasAsTriad();
8596         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8597         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8598         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8599         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8600         final var biasTriad2 = new AccelerationTriad();
8601         calibrator.getBiasAsTriad(biasTriad2);
8602         assertEquals(biasTriad1, biasTriad2);
8603         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8604         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8605         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8606         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8607         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8608         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8609         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8610         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8611         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8612         final var bias1 = calibrator.getBias();
8613         assertArrayEquals(new double[3], bias1, 0.0);
8614         final var bias2 = new double[3];
8615         calibrator.getBias(bias2);
8616         assertArrayEquals(bias1, bias2, 0.0);
8617         final var b1 = calibrator.getBiasAsMatrix();
8618         assertEquals(new Matrix(3, 1), b1);
8619         final var b2 = new Matrix(3, 1);
8620         calibrator.getBiasAsMatrix(b2);
8621         assertEquals(b1, b2);
8622         final var ma1 = calibrator.getInitialMa();
8623         assertEquals(new Matrix(3, 3), ma1);
8624         final var ma2 = new Matrix(3, 3);
8625         calibrator.getInitialMa(ma2);
8626         assertEquals(ma1, ma2);
8627         assertSame(measurements, calibrator.getMeasurements());
8628         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8629                 calibrator.getMeasurementType());
8630         assertFalse(calibrator.isOrderedMeasurementsRequired());
8631         assertFalse(calibrator.isQualityScoresRequired());
8632         assertFalse(calibrator.isCommonAxisUsed());
8633         assertSame(this, calibrator.getListener());
8634         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
8635         assertFalse(calibrator.isReady());
8636         assertFalse(calibrator.isRunning());
8637         assertNull(calibrator.getEstimatedMa());
8638         assertNull(calibrator.getEstimatedSx());
8639         assertNull(calibrator.getEstimatedSy());
8640         assertNull(calibrator.getEstimatedSz());
8641         assertNull(calibrator.getEstimatedMxy());
8642         assertNull(calibrator.getEstimatedMxz());
8643         assertNull(calibrator.getEstimatedMyx());
8644         assertNull(calibrator.getEstimatedMyz());
8645         assertNull(calibrator.getEstimatedMzx());
8646         assertNull(calibrator.getEstimatedMzy());
8647         assertNull(calibrator.getEstimatedCovariance());
8648         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8649         assertNotNull(calibrator.getGroundTruthGravityNorm());
8650         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8651         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8652         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8653                 ABSOLUTE_ERROR));
8654         final var g = new Acceleration(0.0, AccelerationUnit.G);
8655         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8656         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8657         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8658 
8659         // Force IllegalArgumentException
8660         assertThrows(IllegalArgumentException.class,
8661                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, measurements, this));
8662     }
8663 
8664     @Test
8665     void testConstructor83() throws WrongSizeException {
8666         final var randomizer = new UniformRandomizer();
8667         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8668         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8669         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8670         final var nedPosition = new NEDPosition(latitude, longitude, height);
8671         final var nedVelocity = new NEDVelocity();
8672         final var ecefPosition = new ECEFPosition();
8673         final var ecefVelocity = new ECEFVelocity();
8674         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8675         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8676                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8677         final var gravityNorm = gravity.getNorm();
8678 
8679         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true);
8680 
8681         // check default values
8682         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8683         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8684         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8685         final var bx1 = calibrator.getBiasXAsAcceleration();
8686         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8687         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8688         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8689         calibrator.getBiasXAsAcceleration(bx2);
8690         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8691         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8692         final var by1 = calibrator.getBiasYAsAcceleration();
8693         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8694         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8695         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8696         calibrator.getBiasYAsAcceleration(by2);
8697         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8698         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8699         final var bz1 = calibrator.getBiasZAsAcceleration();
8700         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8701         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8702         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8703         calibrator.getBiasZAsAcceleration(bz2);
8704         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8705         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8706         final var biasTriad1 = calibrator.getBiasAsTriad();
8707         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8708         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8709         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8710         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8711         final var biasTriad2 = new AccelerationTriad();
8712         calibrator.getBiasAsTriad(biasTriad2);
8713         assertEquals(biasTriad1, biasTriad2);
8714         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8715         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8716         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8717         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8718         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8719         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8720         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8721         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8722         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8723         final var bias1 = calibrator.getBias();
8724         assertArrayEquals(new double[3], bias1, 0.0);
8725         final var bias2 = new double[3];
8726         calibrator.getBias(bias2);
8727         assertArrayEquals(bias1, bias2, 0.0);
8728         final var b1 = calibrator.getBiasAsMatrix();
8729         assertEquals(new Matrix(3, 1), b1);
8730         final var b2 = new Matrix(3, 1);
8731         calibrator.getBiasAsMatrix(b2);
8732         assertEquals(b1, b2);
8733         final var ma1 = calibrator.getInitialMa();
8734         assertEquals(new Matrix(3, 3), ma1);
8735         final var ma2 = new Matrix(3, 3);
8736         calibrator.getInitialMa(ma2);
8737         assertEquals(ma1, ma2);
8738         assertNull(calibrator.getMeasurements());
8739         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8740                 calibrator.getMeasurementType());
8741         assertFalse(calibrator.isOrderedMeasurementsRequired());
8742         assertFalse(calibrator.isQualityScoresRequired());
8743         assertTrue(calibrator.isCommonAxisUsed());
8744         assertNull(calibrator.getListener());
8745         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8746         assertFalse(calibrator.isReady());
8747         assertFalse(calibrator.isRunning());
8748         assertNull(calibrator.getEstimatedMa());
8749         assertNull(calibrator.getEstimatedSx());
8750         assertNull(calibrator.getEstimatedSy());
8751         assertNull(calibrator.getEstimatedSz());
8752         assertNull(calibrator.getEstimatedMxy());
8753         assertNull(calibrator.getEstimatedMxz());
8754         assertNull(calibrator.getEstimatedMyx());
8755         assertNull(calibrator.getEstimatedMyz());
8756         assertNull(calibrator.getEstimatedMzx());
8757         assertNull(calibrator.getEstimatedMzy());
8758         assertNull(calibrator.getEstimatedCovariance());
8759         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8760         assertNotNull(calibrator.getGroundTruthGravityNorm());
8761         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8762         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8763         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8764                 ABSOLUTE_ERROR));
8765         final var g = new Acceleration(0.0, AccelerationUnit.G);
8766         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8767         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8768         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8769 
8770         // Force IllegalArgumentException
8771         assertThrows(IllegalArgumentException.class,
8772                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, true));
8773     }
8774 
8775     @Test
8776     void testConstructor84() throws WrongSizeException {
8777         final var randomizer = new UniformRandomizer();
8778         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8779         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8780         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8781         final var nedPosition = new NEDPosition(latitude, longitude, height);
8782         final var nedVelocity = new NEDVelocity();
8783         final var ecefPosition = new ECEFPosition();
8784         final var ecefVelocity = new ECEFVelocity();
8785         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8786         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8787                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8788         final var gravityNorm = gravity.getNorm();
8789 
8790         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
8791                 this);
8792 
8793         // check default values
8794         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8795         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8796         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8797         final var bx1 = calibrator.getBiasXAsAcceleration();
8798         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8799         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8800         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8801         calibrator.getBiasXAsAcceleration(bx2);
8802         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8803         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8804         final var by1 = calibrator.getBiasYAsAcceleration();
8805         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8806         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8807         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8808         calibrator.getBiasYAsAcceleration(by2);
8809         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8810         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8811         final var bz1 = calibrator.getBiasZAsAcceleration();
8812         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8813         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8814         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8815         calibrator.getBiasZAsAcceleration(bz2);
8816         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8817         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8818         final var biasTriad1 = calibrator.getBiasAsTriad();
8819         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8820         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8821         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8822         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8823         final var biasTriad2 = new AccelerationTriad();
8824         calibrator.getBiasAsTriad(biasTriad2);
8825         assertEquals(biasTriad1, biasTriad2);
8826         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8827         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8828         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8829         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8830         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8831         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8832         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8833         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8834         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8835         final var bias1 = calibrator.getBias();
8836         assertArrayEquals(new double[3], bias1, 0.0);
8837         final var bias2 = new double[3];
8838         calibrator.getBias(bias2);
8839         assertArrayEquals(bias1, bias2, 0.0);
8840         final var b1 = calibrator.getBiasAsMatrix();
8841         assertEquals(new Matrix(3, 1), b1);
8842         final var b2 = new Matrix(3, 1);
8843         calibrator.getBiasAsMatrix(b2);
8844         assertEquals(b1, b2);
8845         final var ma1 = calibrator.getInitialMa();
8846         assertEquals(new Matrix(3, 3), ma1);
8847         final var ma2 = new Matrix(3, 3);
8848         calibrator.getInitialMa(ma2);
8849         assertEquals(ma1, ma2);
8850         assertNull(calibrator.getMeasurements());
8851         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8852                 calibrator.getMeasurementType());
8853         assertFalse(calibrator.isOrderedMeasurementsRequired());
8854         assertFalse(calibrator.isQualityScoresRequired());
8855         assertTrue(calibrator.isCommonAxisUsed());
8856         assertSame(this, calibrator.getListener());
8857         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8858         assertFalse(calibrator.isReady());
8859         assertFalse(calibrator.isRunning());
8860         assertNull(calibrator.getEstimatedMa());
8861         assertNull(calibrator.getEstimatedSx());
8862         assertNull(calibrator.getEstimatedSy());
8863         assertNull(calibrator.getEstimatedSz());
8864         assertNull(calibrator.getEstimatedMxy());
8865         assertNull(calibrator.getEstimatedMxz());
8866         assertNull(calibrator.getEstimatedMyx());
8867         assertNull(calibrator.getEstimatedMyz());
8868         assertNull(calibrator.getEstimatedMzx());
8869         assertNull(calibrator.getEstimatedMzy());
8870         assertNull(calibrator.getEstimatedCovariance());
8871         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8872         assertNotNull(calibrator.getGroundTruthGravityNorm());
8873         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8874         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8875         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8876                 ABSOLUTE_ERROR));
8877         final var g = new Acceleration(0.0, AccelerationUnit.G);
8878         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8879         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8880         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8881 
8882         // Force IllegalArgumentException
8883         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8884                 -gravityNorm, true, this));
8885     }
8886 
8887     @Test
8888     void testConstructor85() throws WrongSizeException {
8889         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
8890         final var randomizer = new UniformRandomizer();
8891         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
8892         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
8893         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
8894         final var nedPosition = new NEDPosition(latitude, longitude, height);
8895         final var nedVelocity = new NEDVelocity();
8896         final var ecefPosition = new ECEFPosition();
8897         final var ecefVelocity = new ECEFVelocity();
8898         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
8899         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
8900                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
8901         final var gravityNorm = gravity.getNorm();
8902 
8903         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
8904                 true);
8905 
8906         // check default values
8907         assertEquals(0.0, calibrator.getBiasX(), 0.0);
8908         assertEquals(0.0, calibrator.getBiasY(), 0.0);
8909         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
8910         final var bx1 = calibrator.getBiasXAsAcceleration();
8911         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
8912         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
8913         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8914         calibrator.getBiasXAsAcceleration(bx2);
8915         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
8916         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
8917         final var by1 = calibrator.getBiasYAsAcceleration();
8918         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
8919         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
8920         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8921         calibrator.getBiasYAsAcceleration(by2);
8922         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
8923         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
8924         final var bz1 = calibrator.getBiasZAsAcceleration();
8925         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
8926         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
8927         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
8928         calibrator.getBiasZAsAcceleration(bz2);
8929         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
8930         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
8931         final var biasTriad1 = calibrator.getBiasAsTriad();
8932         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
8933         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
8934         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
8935         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
8936         final var biasTriad2 = new AccelerationTriad();
8937         calibrator.getBiasAsTriad(biasTriad2);
8938         assertEquals(biasTriad1, biasTriad2);
8939         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
8940         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
8941         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
8942         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
8943         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
8944         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
8945         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
8946         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
8947         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
8948         final var bias1 = calibrator.getBias();
8949         assertArrayEquals(new double[3], bias1, 0.0);
8950         final var bias2 = new double[3];
8951         calibrator.getBias(bias2);
8952         assertArrayEquals(bias1, bias2, 0.0);
8953         final var b1 = calibrator.getBiasAsMatrix();
8954         assertEquals(new Matrix(3, 1), b1);
8955         final var b2 = new Matrix(3, 1);
8956         calibrator.getBiasAsMatrix(b2);
8957         assertEquals(b1, b2);
8958         final var ma1 = calibrator.getInitialMa();
8959         assertEquals(new Matrix(3, 3), ma1);
8960         final var ma2 = new Matrix(3, 3);
8961         calibrator.getInitialMa(ma2);
8962         assertEquals(ma1, ma2);
8963         assertSame(measurements, calibrator.getMeasurements());
8964         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
8965                 calibrator.getMeasurementType());
8966         assertFalse(calibrator.isOrderedMeasurementsRequired());
8967         assertFalse(calibrator.isQualityScoresRequired());
8968         assertTrue(calibrator.isCommonAxisUsed());
8969         assertNull(calibrator.getListener());
8970         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
8971         assertFalse(calibrator.isReady());
8972         assertFalse(calibrator.isRunning());
8973         assertNull(calibrator.getEstimatedMa());
8974         assertNull(calibrator.getEstimatedSx());
8975         assertNull(calibrator.getEstimatedSy());
8976         assertNull(calibrator.getEstimatedSz());
8977         assertNull(calibrator.getEstimatedMxy());
8978         assertNull(calibrator.getEstimatedMxz());
8979         assertNull(calibrator.getEstimatedMyx());
8980         assertNull(calibrator.getEstimatedMyz());
8981         assertNull(calibrator.getEstimatedMzx());
8982         assertNull(calibrator.getEstimatedMzy());
8983         assertNull(calibrator.getEstimatedCovariance());
8984         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
8985         assertNotNull(calibrator.getGroundTruthGravityNorm());
8986         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
8987         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8988         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
8989                 ABSOLUTE_ERROR));
8990         final var g = new Acceleration(0.0, AccelerationUnit.G);
8991         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
8992         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
8993         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
8994 
8995         // Force IllegalArgumentException
8996         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
8997                 -gravityNorm, measurements, true));
8998     }
8999 
9000     @Test
9001     void testConstructor86() throws WrongSizeException {
9002         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9003         final var randomizer = new UniformRandomizer();
9004         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9005         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9006         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9007         final var nedPosition = new NEDPosition(latitude, longitude, height);
9008         final var nedVelocity = new NEDVelocity();
9009         final var ecefPosition = new ECEFPosition();
9010         final var ecefVelocity = new ECEFVelocity();
9011         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9012         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9013                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9014         final var gravityNorm = gravity.getNorm();
9015 
9016         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9017                 true, this);
9018 
9019         // check default values
9020         assertEquals(0.0, calibrator.getBiasX(), 0.0);
9021         assertEquals(0.0, calibrator.getBiasY(), 0.0);
9022         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
9023         final var bx1 = calibrator.getBiasXAsAcceleration();
9024         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
9025         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9026         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9027         calibrator.getBiasXAsAcceleration(bx2);
9028         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
9029         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9030         final var by1 = calibrator.getBiasYAsAcceleration();
9031         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
9032         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9033         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9034         calibrator.getBiasYAsAcceleration(by2);
9035         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
9036         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9037         final var bz1 = calibrator.getBiasZAsAcceleration();
9038         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
9039         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9040         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9041         calibrator.getBiasZAsAcceleration(bz2);
9042         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
9043         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9044         final var biasTriad1 = calibrator.getBiasAsTriad();
9045         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
9046         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
9047         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
9048         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9049         final var biasTriad2 = new AccelerationTriad();
9050         calibrator.getBiasAsTriad(biasTriad2);
9051         assertEquals(biasTriad1, biasTriad2);
9052         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9053         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9054         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9055         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9056         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9057         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9058         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9059         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9060         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9061         final var bias1 = calibrator.getBias();
9062         assertArrayEquals(new double[3], bias1, 0.0);
9063         final var bias2 = new double[3];
9064         calibrator.getBias(bias2);
9065         assertArrayEquals(bias1, bias2, 0.0);
9066         final var b1 = calibrator.getBiasAsMatrix();
9067         assertEquals(new Matrix(3, 1), b1);
9068         final var b2 = new Matrix(3, 1);
9069         calibrator.getBiasAsMatrix(b2);
9070         assertEquals(b1, b2);
9071         final var ma1 = calibrator.getInitialMa();
9072         assertEquals(new Matrix(3, 3), ma1);
9073         final var ma2 = new Matrix(3, 3);
9074         calibrator.getInitialMa(ma2);
9075         assertEquals(ma1, ma2);
9076         assertSame(measurements, calibrator.getMeasurements());
9077         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9078                 calibrator.getMeasurementType());
9079         assertFalse(calibrator.isOrderedMeasurementsRequired());
9080         assertFalse(calibrator.isQualityScoresRequired());
9081         assertTrue(calibrator.isCommonAxisUsed());
9082         assertSame(this, calibrator.getListener());
9083         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9084         assertFalse(calibrator.isReady());
9085         assertFalse(calibrator.isRunning());
9086         assertNull(calibrator.getEstimatedMa());
9087         assertNull(calibrator.getEstimatedSx());
9088         assertNull(calibrator.getEstimatedSy());
9089         assertNull(calibrator.getEstimatedSz());
9090         assertNull(calibrator.getEstimatedMxy());
9091         assertNull(calibrator.getEstimatedMxz());
9092         assertNull(calibrator.getEstimatedMyx());
9093         assertNull(calibrator.getEstimatedMyz());
9094         assertNull(calibrator.getEstimatedMzx());
9095         assertNull(calibrator.getEstimatedMzy());
9096         assertNull(calibrator.getEstimatedCovariance());
9097         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9098         assertNotNull(calibrator.getGroundTruthGravityNorm());
9099         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9100         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9101         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9102                 ABSOLUTE_ERROR));
9103         final var g = new Acceleration(0.0, AccelerationUnit.G);
9104         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9105         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9106         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9107 
9108         // Force IllegalArgumentException
9109         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9110                 -gravityNorm, measurements, true, this));
9111     }
9112 
9113     @Test
9114     void testConstructor87() throws WrongSizeException {
9115         final var ba = generateBa();
9116         final var biasX = ba.getElementAtIndex(0);
9117         final var biasY = ba.getElementAtIndex(1);
9118         final var biasZ = ba.getElementAtIndex(2);
9119 
9120         final var randomizer = new UniformRandomizer();
9121         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9122         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9123         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9124         final var nedPosition = new NEDPosition(latitude, longitude, height);
9125         final var nedVelocity = new NEDVelocity();
9126         final var ecefPosition = new ECEFPosition();
9127         final var ecefVelocity = new ECEFVelocity();
9128         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9129         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9130                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9131         final var gravityNorm = gravity.getNorm();
9132 
9133         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ);
9134 
9135         // check default values
9136         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9137         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9138         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9139         final var bx1 = calibrator.getBiasXAsAcceleration();
9140         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9141         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9142         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9143         calibrator.getBiasXAsAcceleration(bx2);
9144         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9145         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9146         final var by1 = calibrator.getBiasYAsAcceleration();
9147         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9148         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9149         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9150         calibrator.getBiasYAsAcceleration(by2);
9151         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9152         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9153         final var bz1 = calibrator.getBiasZAsAcceleration();
9154         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9155         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9156         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9157         calibrator.getBiasZAsAcceleration(bz2);
9158         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9159         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9160         final var biasTriad1 = calibrator.getBiasAsTriad();
9161         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9162         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9163         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9164         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9165         final var biasTriad2 = new AccelerationTriad();
9166         calibrator.getBiasAsTriad(biasTriad2);
9167         assertEquals(biasTriad1, biasTriad2);
9168         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9169         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9170         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9171         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9172         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9173         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9174         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9175         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9176         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9177         final var bias1 = calibrator.getBias();
9178         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9179         final var bias2 = new double[3];
9180         calibrator.getBias(bias2);
9181         assertArrayEquals(bias1, bias2, 0.0);
9182         final var b1 = calibrator.getBiasAsMatrix();
9183         assertEquals(b1, ba);
9184         final var b2 = new Matrix(3, 1);
9185         calibrator.getBiasAsMatrix(b2);
9186         assertEquals(b1, b2);
9187         final var ma1 = calibrator.getInitialMa();
9188         assertEquals(new Matrix(3, 3), ma1);
9189         final var ma2 = new Matrix(3, 3);
9190         calibrator.getInitialMa(ma2);
9191         assertEquals(ma1, ma2);
9192         assertNull(calibrator.getMeasurements());
9193         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9194                 calibrator.getMeasurementType());
9195         assertFalse(calibrator.isOrderedMeasurementsRequired());
9196         assertFalse(calibrator.isQualityScoresRequired());
9197         assertFalse(calibrator.isCommonAxisUsed());
9198         assertNull(calibrator.getListener());
9199         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9200         assertFalse(calibrator.isReady());
9201         assertFalse(calibrator.isRunning());
9202         assertNull(calibrator.getEstimatedMa());
9203         assertNull(calibrator.getEstimatedSx());
9204         assertNull(calibrator.getEstimatedSy());
9205         assertNull(calibrator.getEstimatedSz());
9206         assertNull(calibrator.getEstimatedMxy());
9207         assertNull(calibrator.getEstimatedMxz());
9208         assertNull(calibrator.getEstimatedMyx());
9209         assertNull(calibrator.getEstimatedMyz());
9210         assertNull(calibrator.getEstimatedMzx());
9211         assertNull(calibrator.getEstimatedMzy());
9212         assertNull(calibrator.getEstimatedCovariance());
9213         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9214         assertNotNull(calibrator.getGroundTruthGravityNorm());
9215         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9216         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9217         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9218                 ABSOLUTE_ERROR));
9219         final var g = new Acceleration(0.0, AccelerationUnit.G);
9220         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9221         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9222         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9223 
9224         // Force IllegalArgumentException
9225         assertThrows(IllegalArgumentException.class,
9226                 () -> new KnownBiasAndGravityNormAccelerometerCalibrator(-gravityNorm, biasX, biasY, biasZ));
9227     }
9228 
9229     @Test
9230     void testConstructor88() throws WrongSizeException {
9231         final var ba = generateBa();
9232         final var biasX = ba.getElementAtIndex(0);
9233         final var biasY = ba.getElementAtIndex(1);
9234         final var biasZ = ba.getElementAtIndex(2);
9235 
9236         final var randomizer = new UniformRandomizer();
9237         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9238         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9239         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9240         final var nedPosition = new NEDPosition(latitude, longitude, height);
9241         final var nedVelocity = new NEDVelocity();
9242         final var ecefPosition = new ECEFPosition();
9243         final var ecefVelocity = new ECEFVelocity();
9244         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9245         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9246                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9247         final var gravityNorm = gravity.getNorm();
9248 
9249         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
9250                 this);
9251 
9252         // check default values
9253         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9254         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9255         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9256         final var bx1 = calibrator.getBiasXAsAcceleration();
9257         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9258         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9259         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9260         calibrator.getBiasXAsAcceleration(bx2);
9261         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9262         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9263         final var by1 = calibrator.getBiasYAsAcceleration();
9264         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9265         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9266         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9267         calibrator.getBiasYAsAcceleration(by2);
9268         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9269         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9270         final var bz1 = calibrator.getBiasZAsAcceleration();
9271         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9272         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9273         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9274         calibrator.getBiasZAsAcceleration(bz2);
9275         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9276         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9277         final var biasTriad1 = calibrator.getBiasAsTriad();
9278         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9279         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9280         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9281         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9282         final var biasTriad2 = new AccelerationTriad();
9283         calibrator.getBiasAsTriad(biasTriad2);
9284         assertEquals(biasTriad1, biasTriad2);
9285         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9286         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9287         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9288         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9289         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9290         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9291         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9292         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9293         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9294         final var bias1 = calibrator.getBias();
9295         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9296         final var bias2 = new double[3];
9297         calibrator.getBias(bias2);
9298         assertArrayEquals(bias1, bias2, 0.0);
9299         final var b1 = calibrator.getBiasAsMatrix();
9300         assertEquals(b1, ba);
9301         final var b2 = new Matrix(3, 1);
9302         calibrator.getBiasAsMatrix(b2);
9303         assertEquals(b1, b2);
9304         final var ma1 = calibrator.getInitialMa();
9305         assertEquals(new Matrix(3, 3), ma1);
9306         final var ma2 = new Matrix(3, 3);
9307         calibrator.getInitialMa(ma2);
9308         assertEquals(ma1, ma2);
9309         assertNull(calibrator.getMeasurements());
9310         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9311                 calibrator.getMeasurementType());
9312         assertFalse(calibrator.isOrderedMeasurementsRequired());
9313         assertFalse(calibrator.isQualityScoresRequired());
9314         assertFalse(calibrator.isCommonAxisUsed());
9315         assertSame(this, calibrator.getListener());
9316         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9317         assertFalse(calibrator.isReady());
9318         assertFalse(calibrator.isRunning());
9319         assertNull(calibrator.getEstimatedMa());
9320         assertNull(calibrator.getEstimatedSx());
9321         assertNull(calibrator.getEstimatedSy());
9322         assertNull(calibrator.getEstimatedSz());
9323         assertNull(calibrator.getEstimatedMxy());
9324         assertNull(calibrator.getEstimatedMxz());
9325         assertNull(calibrator.getEstimatedMyx());
9326         assertNull(calibrator.getEstimatedMyz());
9327         assertNull(calibrator.getEstimatedMzx());
9328         assertNull(calibrator.getEstimatedMzy());
9329         assertNull(calibrator.getEstimatedCovariance());
9330         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9331         assertNotNull(calibrator.getGroundTruthGravityNorm());
9332         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9333         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9334         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9335                 ABSOLUTE_ERROR));
9336         final var g = new Acceleration(0.0, AccelerationUnit.G);
9337         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9338         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9339         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9340 
9341         // Force IllegalArgumentException
9342         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9343                 -gravityNorm, biasX, biasY, biasZ, this));
9344     }
9345 
9346     @Test
9347     void testConstructor89() throws WrongSizeException {
9348         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9349 
9350         final var ba = generateBa();
9351         final var biasX = ba.getElementAtIndex(0);
9352         final var biasY = ba.getElementAtIndex(1);
9353         final var biasZ = ba.getElementAtIndex(2);
9354 
9355         final var randomizer = new UniformRandomizer();
9356         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9357         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9358         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9359         final var nedPosition = new NEDPosition(latitude, longitude, height);
9360         final var nedVelocity = new NEDVelocity();
9361         final var ecefPosition = new ECEFPosition();
9362         final var ecefVelocity = new ECEFVelocity();
9363         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9364         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9365                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9366         final var gravityNorm = gravity.getNorm();
9367 
9368         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9369                 biasX, biasY, biasZ);
9370 
9371         // check default values
9372         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9373         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9374         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9375         final var bx1 = calibrator.getBiasXAsAcceleration();
9376         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9377         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9378         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9379         calibrator.getBiasXAsAcceleration(bx2);
9380         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9381         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9382         final var by1 = calibrator.getBiasYAsAcceleration();
9383         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9384         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9385         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9386         calibrator.getBiasYAsAcceleration(by2);
9387         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9388         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9389         final var bz1 = calibrator.getBiasZAsAcceleration();
9390         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9391         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9392         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9393         calibrator.getBiasZAsAcceleration(bz2);
9394         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9395         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9396         final var biasTriad1 = calibrator.getBiasAsTriad();
9397         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9398         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9399         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9400         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9401         final var biasTriad2 = new AccelerationTriad();
9402         calibrator.getBiasAsTriad(biasTriad2);
9403         assertEquals(biasTriad1, biasTriad2);
9404         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9405         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9406         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9407         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9408         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9409         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9410         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9411         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9412         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9413         final var bias1 = calibrator.getBias();
9414         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9415         final var bias2 = new double[3];
9416         calibrator.getBias(bias2);
9417         assertArrayEquals(bias1, bias2, 0.0);
9418         final var b1 = calibrator.getBiasAsMatrix();
9419         assertEquals(b1, ba);
9420         final var b2 = new Matrix(3, 1);
9421         calibrator.getBiasAsMatrix(b2);
9422         assertEquals(b1, b2);
9423         final var ma1 = calibrator.getInitialMa();
9424         assertEquals(new Matrix(3, 3), ma1);
9425         final var ma2 = new Matrix(3, 3);
9426         calibrator.getInitialMa(ma2);
9427         assertEquals(ma1, ma2);
9428         assertSame(measurements, calibrator.getMeasurements());
9429         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9430                 calibrator.getMeasurementType());
9431         assertFalse(calibrator.isOrderedMeasurementsRequired());
9432         assertFalse(calibrator.isQualityScoresRequired());
9433         assertFalse(calibrator.isCommonAxisUsed());
9434         assertNull(calibrator.getListener());
9435         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9436         assertFalse(calibrator.isReady());
9437         assertFalse(calibrator.isRunning());
9438         assertNull(calibrator.getEstimatedMa());
9439         assertNull(calibrator.getEstimatedSx());
9440         assertNull(calibrator.getEstimatedSy());
9441         assertNull(calibrator.getEstimatedSz());
9442         assertNull(calibrator.getEstimatedMxy());
9443         assertNull(calibrator.getEstimatedMxz());
9444         assertNull(calibrator.getEstimatedMyx());
9445         assertNull(calibrator.getEstimatedMyz());
9446         assertNull(calibrator.getEstimatedMzx());
9447         assertNull(calibrator.getEstimatedMzy());
9448         assertNull(calibrator.getEstimatedCovariance());
9449         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9450         assertNotNull(calibrator.getGroundTruthGravityNorm());
9451         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9452         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9453         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9454                 ABSOLUTE_ERROR));
9455         final var g = new Acceleration(0.0, AccelerationUnit.G);
9456         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9457         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9458         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9459 
9460         // Force IllegalArgumentException
9461         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9462                 -gravityNorm, measurements, biasX, biasY, biasZ));
9463     }
9464 
9465     @Test
9466     void testConstructor90() throws WrongSizeException {
9467         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9468 
9469         final var ba = generateBa();
9470         final var biasX = ba.getElementAtIndex(0);
9471         final var biasY = ba.getElementAtIndex(1);
9472         final var biasZ = ba.getElementAtIndex(2);
9473 
9474         final var randomizer = new UniformRandomizer();
9475         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9476         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9477         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9478         final var nedPosition = new NEDPosition(latitude, longitude, height);
9479         final var nedVelocity = new NEDVelocity();
9480         final var ecefPosition = new ECEFPosition();
9481         final var ecefVelocity = new ECEFVelocity();
9482         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9483         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9484                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9485         final var gravityNorm = gravity.getNorm();
9486 
9487         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9488                 biasX, biasY, biasZ, this);
9489 
9490         // check default values
9491         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9492         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9493         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9494         final var bx1 = calibrator.getBiasXAsAcceleration();
9495         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9496         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9497         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9498         calibrator.getBiasXAsAcceleration(bx2);
9499         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9500         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9501         final var by1 = calibrator.getBiasYAsAcceleration();
9502         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9503         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9504         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9505         calibrator.getBiasYAsAcceleration(by2);
9506         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9507         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9508         final var bz1 = calibrator.getBiasZAsAcceleration();
9509         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9510         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9511         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9512         calibrator.getBiasZAsAcceleration(bz2);
9513         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9514         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9515         final var biasTriad1 = calibrator.getBiasAsTriad();
9516         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9517         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9518         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9519         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9520         final var biasTriad2 = new AccelerationTriad();
9521         calibrator.getBiasAsTriad(biasTriad2);
9522         assertEquals(biasTriad1, biasTriad2);
9523         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9524         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9525         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9526         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9527         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9528         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9529         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9530         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9531         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9532         final var bias1 = calibrator.getBias();
9533         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9534         final var bias2 = new double[3];
9535         calibrator.getBias(bias2);
9536         assertArrayEquals(bias1, bias2, 0.0);
9537         final var b1 = calibrator.getBiasAsMatrix();
9538         assertEquals(b1, ba);
9539         final var b2 = new Matrix(3, 1);
9540         calibrator.getBiasAsMatrix(b2);
9541         assertEquals(b1, b2);
9542         final var ma1 = calibrator.getInitialMa();
9543         assertEquals(new Matrix(3, 3), ma1);
9544         final var ma2 = new Matrix(3, 3);
9545         calibrator.getInitialMa(ma2);
9546         assertEquals(ma1, ma2);
9547         assertSame(measurements, calibrator.getMeasurements());
9548         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9549                 calibrator.getMeasurementType());
9550         assertFalse(calibrator.isOrderedMeasurementsRequired());
9551         assertFalse(calibrator.isQualityScoresRequired());
9552         assertFalse(calibrator.isCommonAxisUsed());
9553         assertSame(this, calibrator.getListener());
9554         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
9555         assertFalse(calibrator.isReady());
9556         assertFalse(calibrator.isRunning());
9557         assertNull(calibrator.getEstimatedMa());
9558         assertNull(calibrator.getEstimatedSx());
9559         assertNull(calibrator.getEstimatedSy());
9560         assertNull(calibrator.getEstimatedSz());
9561         assertNull(calibrator.getEstimatedMxy());
9562         assertNull(calibrator.getEstimatedMxz());
9563         assertNull(calibrator.getEstimatedMyx());
9564         assertNull(calibrator.getEstimatedMyz());
9565         assertNull(calibrator.getEstimatedMzx());
9566         assertNull(calibrator.getEstimatedMzy());
9567         assertNull(calibrator.getEstimatedCovariance());
9568         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9569         assertNotNull(calibrator.getGroundTruthGravityNorm());
9570         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9571         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9572         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9573                 ABSOLUTE_ERROR));
9574         final var g = new Acceleration(0.0, AccelerationUnit.G);
9575         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9576         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9577         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9578 
9579         // Force IllegalArgumentException
9580         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9581                 -gravityNorm, measurements, biasX, biasY, biasZ, this));
9582     }
9583 
9584     @Test
9585     void testConstructor91() throws WrongSizeException {
9586         final var ba = generateBa();
9587         final var biasX = ba.getElementAtIndex(0);
9588         final var biasY = ba.getElementAtIndex(1);
9589         final var biasZ = ba.getElementAtIndex(2);
9590 
9591         final var randomizer = new UniformRandomizer();
9592         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9593         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9594         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9595         final var nedPosition = new NEDPosition(latitude, longitude, height);
9596         final var nedVelocity = new NEDVelocity();
9597         final var ecefPosition = new ECEFPosition();
9598         final var ecefVelocity = new ECEFVelocity();
9599         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9600         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9601                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9602         final var gravityNorm = gravity.getNorm();
9603 
9604         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
9605                 biasX, biasY, biasZ);
9606 
9607         // check default values
9608         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9609         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9610         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9611         final var bx1 = calibrator.getBiasXAsAcceleration();
9612         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9613         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9614         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9615         calibrator.getBiasXAsAcceleration(bx2);
9616         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9617         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9618         final var by1 = calibrator.getBiasYAsAcceleration();
9619         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9620         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9621         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9622         calibrator.getBiasYAsAcceleration(by2);
9623         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9624         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9625         final var bz1 = calibrator.getBiasZAsAcceleration();
9626         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9627         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9628         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9629         calibrator.getBiasZAsAcceleration(bz2);
9630         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9631         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9632         final var biasTriad1 = calibrator.getBiasAsTriad();
9633         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9634         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9635         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9636         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9637         final var biasTriad2 = new AccelerationTriad();
9638         calibrator.getBiasAsTriad(biasTriad2);
9639         assertEquals(biasTriad1, biasTriad2);
9640         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9641         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9642         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9643         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9644         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9645         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9646         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9647         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9648         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9649         final var bias1 = calibrator.getBias();
9650         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9651         final var bias2 = new double[3];
9652         calibrator.getBias(bias2);
9653         assertArrayEquals(bias1, bias2, 0.0);
9654         final var b1 = calibrator.getBiasAsMatrix();
9655         assertEquals(b1, ba);
9656         final var b2 = new Matrix(3, 1);
9657         calibrator.getBiasAsMatrix(b2);
9658         assertEquals(b1, b2);
9659         final var ma1 = calibrator.getInitialMa();
9660         assertEquals(new Matrix(3, 3), ma1);
9661         final var ma2 = new Matrix(3, 3);
9662         calibrator.getInitialMa(ma2);
9663         assertEquals(ma1, ma2);
9664         assertNull(calibrator.getMeasurements());
9665         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9666                 calibrator.getMeasurementType());
9667         assertFalse(calibrator.isOrderedMeasurementsRequired());
9668         assertFalse(calibrator.isQualityScoresRequired());
9669         assertTrue(calibrator.isCommonAxisUsed());
9670         assertNull(calibrator.getListener());
9671         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9672         assertFalse(calibrator.isReady());
9673         assertFalse(calibrator.isRunning());
9674         assertNull(calibrator.getEstimatedMa());
9675         assertNull(calibrator.getEstimatedSx());
9676         assertNull(calibrator.getEstimatedSy());
9677         assertNull(calibrator.getEstimatedSz());
9678         assertNull(calibrator.getEstimatedMxy());
9679         assertNull(calibrator.getEstimatedMxz());
9680         assertNull(calibrator.getEstimatedMyx());
9681         assertNull(calibrator.getEstimatedMyz());
9682         assertNull(calibrator.getEstimatedMzx());
9683         assertNull(calibrator.getEstimatedMzy());
9684         assertNull(calibrator.getEstimatedCovariance());
9685         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9686         assertNotNull(calibrator.getGroundTruthGravityNorm());
9687         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9688         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9689         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9690                 ABSOLUTE_ERROR));
9691         final var g = new Acceleration(0.0, AccelerationUnit.G);
9692         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9693         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9694         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9695 
9696         // Force IllegalArgumentException
9697         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9698                 -gravityNorm, true, biasX, biasY, biasZ));
9699     }
9700 
9701     @Test
9702     void testConstructor92() throws WrongSizeException {
9703         final var ba = generateBa();
9704         final var biasX = ba.getElementAtIndex(0);
9705         final var biasY = ba.getElementAtIndex(1);
9706         final var biasZ = ba.getElementAtIndex(2);
9707 
9708         final var randomizer = new UniformRandomizer();
9709         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9710         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9711         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9712         final var nedPosition = new NEDPosition(latitude, longitude, height);
9713         final var nedVelocity = new NEDVelocity();
9714         final var ecefPosition = new ECEFPosition();
9715         final var ecefVelocity = new ECEFVelocity();
9716         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9717         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9718                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9719         final var gravityNorm = gravity.getNorm();
9720 
9721         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
9722                 biasX, biasY, biasZ, this);
9723 
9724         // check default values
9725         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9726         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9727         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9728         final var bx1 = calibrator.getBiasXAsAcceleration();
9729         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9730         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9731         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9732         calibrator.getBiasXAsAcceleration(bx2);
9733         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9734         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9735         final var by1 = calibrator.getBiasYAsAcceleration();
9736         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9737         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9738         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9739         calibrator.getBiasYAsAcceleration(by2);
9740         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9741         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9742         final var bz1 = calibrator.getBiasZAsAcceleration();
9743         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9744         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9745         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9746         calibrator.getBiasZAsAcceleration(bz2);
9747         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9748         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9749         final var biasTriad1 = calibrator.getBiasAsTriad();
9750         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9751         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9752         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9753         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9754         final var biasTriad2 = new AccelerationTriad();
9755         calibrator.getBiasAsTriad(biasTriad2);
9756         assertEquals(biasTriad1, biasTriad2);
9757         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9758         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9759         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9760         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9761         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9762         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9763         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9764         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9765         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9766         final var bias1 = calibrator.getBias();
9767         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9768         final var bias2 = new double[3];
9769         calibrator.getBias(bias2);
9770         assertArrayEquals(bias1, bias2, 0.0);
9771         final var b1 = calibrator.getBiasAsMatrix();
9772         assertEquals(b1, ba);
9773         final var b2 = new Matrix(3, 1);
9774         calibrator.getBiasAsMatrix(b2);
9775         assertEquals(b1, b2);
9776         final var ma1 = calibrator.getInitialMa();
9777         assertEquals(new Matrix(3, 3), ma1);
9778         final var ma2 = new Matrix(3, 3);
9779         calibrator.getInitialMa(ma2);
9780         assertEquals(ma1, ma2);
9781         assertNull(calibrator.getMeasurements());
9782         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9783                 calibrator.getMeasurementType());
9784         assertFalse(calibrator.isOrderedMeasurementsRequired());
9785         assertFalse(calibrator.isQualityScoresRequired());
9786         assertTrue(calibrator.isCommonAxisUsed());
9787         assertSame(this, calibrator.getListener());
9788         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9789         assertFalse(calibrator.isReady());
9790         assertFalse(calibrator.isRunning());
9791         assertNull(calibrator.getEstimatedMa());
9792         assertNull(calibrator.getEstimatedSx());
9793         assertNull(calibrator.getEstimatedSy());
9794         assertNull(calibrator.getEstimatedSz());
9795         assertNull(calibrator.getEstimatedMxy());
9796         assertNull(calibrator.getEstimatedMxz());
9797         assertNull(calibrator.getEstimatedMyx());
9798         assertNull(calibrator.getEstimatedMyz());
9799         assertNull(calibrator.getEstimatedMzx());
9800         assertNull(calibrator.getEstimatedMzy());
9801         assertNull(calibrator.getEstimatedCovariance());
9802         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9803         assertNotNull(calibrator.getGroundTruthGravityNorm());
9804         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9805         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9806         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9807                 ABSOLUTE_ERROR));
9808         final var g = new Acceleration(0.0, AccelerationUnit.G);
9809         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9810         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9811         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9812 
9813         // Force IllegalArgumentException
9814         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9815                 -gravityNorm, true, biasX, biasY, biasZ, this));
9816     }
9817 
9818     @Test
9819     void testConstructor93() throws WrongSizeException {
9820         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9821 
9822         final var ba = generateBa();
9823         final var biasX = ba.getElementAtIndex(0);
9824         final var biasY = ba.getElementAtIndex(1);
9825         final var biasZ = ba.getElementAtIndex(2);
9826 
9827         final var randomizer = new UniformRandomizer();
9828         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9829         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9830         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9831         final var nedPosition = new NEDPosition(latitude, longitude, height);
9832         final var nedVelocity = new NEDVelocity();
9833         final var ecefPosition = new ECEFPosition();
9834         final var ecefVelocity = new ECEFVelocity();
9835         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9836         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9837                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9838         final var gravityNorm = gravity.getNorm();
9839 
9840         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9841                 true, biasX, biasY, biasZ);
9842 
9843         // check default values
9844         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9845         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9846         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9847         final var bx1 = calibrator.getBiasXAsAcceleration();
9848         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9849         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9850         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9851         calibrator.getBiasXAsAcceleration(bx2);
9852         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9853         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9854         final var by1 = calibrator.getBiasYAsAcceleration();
9855         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9856         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9857         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9858         calibrator.getBiasYAsAcceleration(by2);
9859         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9860         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9861         final var bz1 = calibrator.getBiasZAsAcceleration();
9862         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9863         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9864         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9865         calibrator.getBiasZAsAcceleration(bz2);
9866         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9867         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9868         final var biasTriad1 = calibrator.getBiasAsTriad();
9869         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9870         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9871         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9872         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9873         final var biasTriad2 = new AccelerationTriad();
9874         calibrator.getBiasAsTriad(biasTriad2);
9875         assertEquals(biasTriad1, biasTriad2);
9876         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9877         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9878         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9879         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9880         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9881         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
9882         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
9883         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
9884         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
9885         final var bias1 = calibrator.getBias();
9886         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9887         final var bias2 = new double[3];
9888         calibrator.getBias(bias2);
9889         assertArrayEquals(bias1, bias2, 0.0);
9890         final var b1 = calibrator.getBiasAsMatrix();
9891         assertEquals(b1, ba);
9892         final var b2 = new Matrix(3, 1);
9893         calibrator.getBiasAsMatrix(b2);
9894         assertEquals(b1, b2);
9895         final var ma1 = calibrator.getInitialMa();
9896         assertEquals(new Matrix(3, 3), ma1);
9897         final var ma2 = new Matrix(3, 3);
9898         calibrator.getInitialMa(ma2);
9899         assertEquals(ma1, ma2);
9900         assertSame(measurements, calibrator.getMeasurements());
9901         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
9902                 calibrator.getMeasurementType());
9903         assertFalse(calibrator.isOrderedMeasurementsRequired());
9904         assertFalse(calibrator.isQualityScoresRequired());
9905         assertTrue(calibrator.isCommonAxisUsed());
9906         assertNull(calibrator.getListener());
9907         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
9908         assertFalse(calibrator.isReady());
9909         assertFalse(calibrator.isRunning());
9910         assertNull(calibrator.getEstimatedMa());
9911         assertNull(calibrator.getEstimatedSx());
9912         assertNull(calibrator.getEstimatedSy());
9913         assertNull(calibrator.getEstimatedSz());
9914         assertNull(calibrator.getEstimatedMxy());
9915         assertNull(calibrator.getEstimatedMxz());
9916         assertNull(calibrator.getEstimatedMyx());
9917         assertNull(calibrator.getEstimatedMyz());
9918         assertNull(calibrator.getEstimatedMzx());
9919         assertNull(calibrator.getEstimatedMzy());
9920         assertNull(calibrator.getEstimatedCovariance());
9921         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
9922         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9923         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9924         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
9925                 ABSOLUTE_ERROR));
9926         final var g = new Acceleration(0.0, AccelerationUnit.G);
9927         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9928         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9929         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
9930 
9931         // Force IllegalArgumentException
9932         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
9933                 -gravityNorm, measurements, true, biasX, biasY, biasZ));
9934     }
9935 
9936     @Test
9937     void testConstructor94() throws WrongSizeException {
9938         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
9939 
9940         final var ba = generateBa();
9941         final var biasX = ba.getElementAtIndex(0);
9942         final var biasY = ba.getElementAtIndex(1);
9943         final var biasZ = ba.getElementAtIndex(2);
9944 
9945         final var randomizer = new UniformRandomizer();
9946         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
9947         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9948         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9949         final var nedPosition = new NEDPosition(latitude, longitude, height);
9950         final var nedVelocity = new NEDVelocity();
9951         final var ecefPosition = new ECEFPosition();
9952         final var ecefVelocity = new ECEFVelocity();
9953         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
9954         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9955                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9956         final var gravityNorm = gravity.getNorm();
9957 
9958         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
9959                 true, biasX, biasY, biasZ, this);
9960 
9961         // check default values
9962         assertEquals(biasX, calibrator.getBiasX(), 0.0);
9963         assertEquals(biasY, calibrator.getBiasY(), 0.0);
9964         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
9965         final var bx1 = calibrator.getBiasXAsAcceleration();
9966         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
9967         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
9968         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9969         calibrator.getBiasXAsAcceleration(bx2);
9970         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
9971         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
9972         final var by1 = calibrator.getBiasYAsAcceleration();
9973         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
9974         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
9975         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9976         calibrator.getBiasYAsAcceleration(by2);
9977         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
9978         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
9979         final var bz1 = calibrator.getBiasZAsAcceleration();
9980         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
9981         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
9982         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
9983         calibrator.getBiasZAsAcceleration(bz2);
9984         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
9985         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
9986         final var biasTriad1 = calibrator.getBiasAsTriad();
9987         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
9988         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
9989         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
9990         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
9991         final var biasTriad2 = new AccelerationTriad();
9992         calibrator.getBiasAsTriad(biasTriad2);
9993         assertEquals(biasTriad1, biasTriad2);
9994         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
9995         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
9996         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
9997         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
9998         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
9999         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10000         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10001         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10002         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10003         final var bias1 = calibrator.getBias();
10004         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10005         final var bias2 = new double[3];
10006         calibrator.getBias(bias2);
10007         assertArrayEquals(bias1, bias2, 0.0);
10008         final var b1 = calibrator.getBiasAsMatrix();
10009         assertEquals(b1, ba);
10010         final var b2 = new Matrix(3, 1);
10011         calibrator.getBiasAsMatrix(b2);
10012         assertEquals(b1, b2);
10013         final var ma1 = calibrator.getInitialMa();
10014         assertEquals(new Matrix(3, 3), ma1);
10015         final var ma2 = new Matrix(3, 3);
10016         calibrator.getInitialMa(ma2);
10017         assertEquals(ma1, ma2);
10018         assertSame(measurements, calibrator.getMeasurements());
10019         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10020                 calibrator.getMeasurementType());
10021         assertFalse(calibrator.isOrderedMeasurementsRequired());
10022         assertFalse(calibrator.isQualityScoresRequired());
10023         assertTrue(calibrator.isCommonAxisUsed());
10024         assertSame(this, calibrator.getListener());
10025         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10026         assertFalse(calibrator.isReady());
10027         assertFalse(calibrator.isRunning());
10028         assertNull(calibrator.getEstimatedMa());
10029         assertNull(calibrator.getEstimatedSx());
10030         assertNull(calibrator.getEstimatedSy());
10031         assertNull(calibrator.getEstimatedSz());
10032         assertNull(calibrator.getEstimatedMxy());
10033         assertNull(calibrator.getEstimatedMxz());
10034         assertNull(calibrator.getEstimatedMyx());
10035         assertNull(calibrator.getEstimatedMyz());
10036         assertNull(calibrator.getEstimatedMzx());
10037         assertNull(calibrator.getEstimatedMzy());
10038         assertNull(calibrator.getEstimatedCovariance());
10039         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10040         assertNotNull(calibrator.getGroundTruthGravityNorm());
10041         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10042         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10043         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10044                 ABSOLUTE_ERROR));
10045         final var g = new Acceleration(0.0, AccelerationUnit.G);
10046         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10047         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10048         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10049 
10050         // Force IllegalArgumentException
10051         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10052                 -gravityNorm, measurements, true, biasX, biasY, biasZ, this));
10053     }
10054 
10055     @Test
10056     void testConstructor95() throws WrongSizeException {
10057         final var ba = generateBa();
10058         final var biasX = ba.getElementAtIndex(0);
10059         final var biasY = ba.getElementAtIndex(1);
10060         final var biasZ = ba.getElementAtIndex(2);
10061 
10062         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10063         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10064         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10065 
10066         final var randomizer = new UniformRandomizer();
10067         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10068         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10069         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10070         final var nedPosition = new NEDPosition(latitude, longitude, height);
10071         final var nedVelocity = new NEDVelocity();
10072         final var ecefPosition = new ECEFPosition();
10073         final var ecefVelocity = new ECEFVelocity();
10074         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10075         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10076                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10077         final var gravityNorm = gravity.getNorm();
10078 
10079         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz);
10080 
10081         // check default values
10082         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10083         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10084         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10085         final var bx1 = calibrator.getBiasXAsAcceleration();
10086         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10087         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10088         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10089         calibrator.getBiasXAsAcceleration(bx2);
10090         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10091         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10092         final var by1 = calibrator.getBiasYAsAcceleration();
10093         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10094         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10095         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10096         calibrator.getBiasYAsAcceleration(by2);
10097         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10098         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10099         final var bz1 = calibrator.getBiasZAsAcceleration();
10100         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10101         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10102         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10103         calibrator.getBiasZAsAcceleration(bz2);
10104         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10105         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10106         final var biasTriad1 = calibrator.getBiasAsTriad();
10107         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10108         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10109         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10110         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10111         final var biasTriad2 = new AccelerationTriad();
10112         calibrator.getBiasAsTriad(biasTriad2);
10113         assertEquals(biasTriad1, biasTriad2);
10114         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10115         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10116         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10117         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10118         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10119         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10120         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10121         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10122         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10123         final var bias1 = calibrator.getBias();
10124         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10125         final var bias2 = new double[3];
10126         calibrator.getBias(bias2);
10127         assertArrayEquals(bias1, bias2, 0.0);
10128         final var b1 = calibrator.getBiasAsMatrix();
10129         assertEquals(b1, ba);
10130         final var b2 = new Matrix(3, 1);
10131         calibrator.getBiasAsMatrix(b2);
10132         assertEquals(b1, b2);
10133         final var ma1 = calibrator.getInitialMa();
10134         assertEquals(new Matrix(3, 3), ma1);
10135         final var ma2 = new Matrix(3, 3);
10136         calibrator.getInitialMa(ma2);
10137         assertEquals(ma1, ma2);
10138         assertNull(calibrator.getMeasurements());
10139         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10140                 calibrator.getMeasurementType());
10141         assertFalse(calibrator.isOrderedMeasurementsRequired());
10142         assertFalse(calibrator.isQualityScoresRequired());
10143         assertFalse(calibrator.isCommonAxisUsed());
10144         assertNull(calibrator.getListener());
10145         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10146         assertFalse(calibrator.isReady());
10147         assertFalse(calibrator.isRunning());
10148         assertNull(calibrator.getEstimatedMa());
10149         assertNull(calibrator.getEstimatedSx());
10150         assertNull(calibrator.getEstimatedSy());
10151         assertNull(calibrator.getEstimatedSz());
10152         assertNull(calibrator.getEstimatedMxy());
10153         assertNull(calibrator.getEstimatedMxz());
10154         assertNull(calibrator.getEstimatedMyx());
10155         assertNull(calibrator.getEstimatedMyz());
10156         assertNull(calibrator.getEstimatedMzx());
10157         assertNull(calibrator.getEstimatedMzy());
10158         assertNull(calibrator.getEstimatedCovariance());
10159         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10160         assertNotNull(calibrator.getGroundTruthGravityNorm());
10161         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10162         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10163         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10164                 ABSOLUTE_ERROR));
10165         final var g = new Acceleration(0.0, AccelerationUnit.G);
10166         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10167         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10168         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10169 
10170         // Force IllegalArgumentException
10171         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10172                 -gravityNorm, bx, by, bz));
10173     }
10174 
10175     @Test
10176     void testConstructor96() throws WrongSizeException {
10177         final var ba = generateBa();
10178         final var biasX = ba.getElementAtIndex(0);
10179         final var biasY = ba.getElementAtIndex(1);
10180         final var biasZ = ba.getElementAtIndex(2);
10181 
10182         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10183         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10184         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10185 
10186         final var randomizer = new UniformRandomizer();
10187         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10188         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10189         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10190         final var nedPosition = new NEDPosition(latitude, longitude, height);
10191         final var nedVelocity = new NEDVelocity();
10192         final var ecefPosition = new ECEFPosition();
10193         final var ecefVelocity = new ECEFVelocity();
10194         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10195         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10196                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10197         final var gravityNorm = gravity.getNorm();
10198 
10199         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz,
10200                 this);
10201 
10202         // check default values
10203         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10204         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10205         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10206         final var bx1 = calibrator.getBiasXAsAcceleration();
10207         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10208         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10209         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10210         calibrator.getBiasXAsAcceleration(bx2);
10211         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10212         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10213         final var by1 = calibrator.getBiasYAsAcceleration();
10214         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10215         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10216         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10217         calibrator.getBiasYAsAcceleration(by2);
10218         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10219         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10220         final var bz1 = calibrator.getBiasZAsAcceleration();
10221         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10222         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10223         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10224         calibrator.getBiasZAsAcceleration(bz2);
10225         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10226         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10227         final var biasTriad1 = calibrator.getBiasAsTriad();
10228         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10229         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10230         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10231         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10232         final var biasTriad2 = new AccelerationTriad();
10233         calibrator.getBiasAsTriad(biasTriad2);
10234         assertEquals(biasTriad1, biasTriad2);
10235         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10236         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10237         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10238         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10239         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10240         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10241         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10242         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10243         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10244         final var bias1 = calibrator.getBias();
10245         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10246         final var bias2 = new double[3];
10247         calibrator.getBias(bias2);
10248         assertArrayEquals(bias1, bias2, 0.0);
10249         final var b1 = calibrator.getBiasAsMatrix();
10250         assertEquals(b1, ba);
10251         final var b2 = new Matrix(3, 1);
10252         calibrator.getBiasAsMatrix(b2);
10253         assertEquals(b1, b2);
10254         final var ma1 = calibrator.getInitialMa();
10255         assertEquals(new Matrix(3, 3), ma1);
10256         final var ma2 = new Matrix(3, 3);
10257         calibrator.getInitialMa(ma2);
10258         assertEquals(ma1, ma2);
10259         assertNull(calibrator.getMeasurements());
10260         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10261                 calibrator.getMeasurementType());
10262         assertFalse(calibrator.isOrderedMeasurementsRequired());
10263         assertFalse(calibrator.isQualityScoresRequired());
10264         assertFalse(calibrator.isCommonAxisUsed());
10265         assertSame(this, calibrator.getListener());
10266         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10267         assertFalse(calibrator.isReady());
10268         assertFalse(calibrator.isRunning());
10269         assertNull(calibrator.getEstimatedMa());
10270         assertNull(calibrator.getEstimatedSx());
10271         assertNull(calibrator.getEstimatedSy());
10272         assertNull(calibrator.getEstimatedSz());
10273         assertNull(calibrator.getEstimatedMxy());
10274         assertNull(calibrator.getEstimatedMxz());
10275         assertNull(calibrator.getEstimatedMyx());
10276         assertNull(calibrator.getEstimatedMyz());
10277         assertNull(calibrator.getEstimatedMzx());
10278         assertNull(calibrator.getEstimatedMzy());
10279         assertNull(calibrator.getEstimatedCovariance());
10280         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10281         assertNotNull(calibrator.getGroundTruthGravityNorm());
10282         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10283         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10284         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10285                 ABSOLUTE_ERROR));
10286         final var g = new Acceleration(0.0, AccelerationUnit.G);
10287         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10288         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10289         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10290 
10291         // Force IllegalArgumentException
10292         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10293                 -gravityNorm, bx, by, bz, this));
10294     }
10295 
10296     @Test
10297     void testConstructor97() throws WrongSizeException {
10298         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10299 
10300         final var ba = generateBa();
10301         final var biasX = ba.getElementAtIndex(0);
10302         final var biasY = ba.getElementAtIndex(1);
10303         final var biasZ = ba.getElementAtIndex(2);
10304 
10305         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10306         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10307         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10308 
10309         final var randomizer = new UniformRandomizer();
10310         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10311         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10312         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10313         final var nedPosition = new NEDPosition(latitude, longitude, height);
10314         final var nedVelocity = new NEDVelocity();
10315         final var ecefPosition = new ECEFPosition();
10316         final var ecefVelocity = new ECEFVelocity();
10317         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10318         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10319                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10320         final var gravityNorm = gravity.getNorm();
10321 
10322         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
10323                 bx, by, bz);
10324 
10325         // check default values
10326         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10327         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10328         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10329         final var bx1 = calibrator.getBiasXAsAcceleration();
10330         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10331         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10332         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10333         calibrator.getBiasXAsAcceleration(bx2);
10334         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10335         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10336         final var by1 = calibrator.getBiasYAsAcceleration();
10337         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10338         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10339         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10340         calibrator.getBiasYAsAcceleration(by2);
10341         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10342         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10343         final var bz1 = calibrator.getBiasZAsAcceleration();
10344         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10345         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10346         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10347         calibrator.getBiasZAsAcceleration(bz2);
10348         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10349         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10350         final var biasTriad1 = calibrator.getBiasAsTriad();
10351         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10352         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10353         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10354         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10355         final var biasTriad2 = new AccelerationTriad();
10356         calibrator.getBiasAsTriad(biasTriad2);
10357         assertEquals(biasTriad1, biasTriad2);
10358         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10359         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10360         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10361         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10362         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10363         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10364         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10365         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10366         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10367         final var bias1 = calibrator.getBias();
10368         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10369         final var bias2 = new double[3];
10370         calibrator.getBias(bias2);
10371         assertArrayEquals(bias1, bias2, 0.0);
10372         final var b1 = calibrator.getBiasAsMatrix();
10373         assertEquals(b1, ba);
10374         final var b2 = new Matrix(3, 1);
10375         calibrator.getBiasAsMatrix(b2);
10376         assertEquals(b1, b2);
10377         final var ma1 = calibrator.getInitialMa();
10378         assertEquals(new Matrix(3, 3), ma1);
10379         final var ma2 = new Matrix(3, 3);
10380         calibrator.getInitialMa(ma2);
10381         assertEquals(ma1, ma2);
10382         assertSame(measurements, calibrator.getMeasurements());
10383         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10384                 calibrator.getMeasurementType());
10385         assertFalse(calibrator.isOrderedMeasurementsRequired());
10386         assertFalse(calibrator.isQualityScoresRequired());
10387         assertFalse(calibrator.isCommonAxisUsed());
10388         assertNull(calibrator.getListener());
10389         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10390         assertFalse(calibrator.isReady());
10391         assertFalse(calibrator.isRunning());
10392         assertNull(calibrator.getEstimatedMa());
10393         assertNull(calibrator.getEstimatedSx());
10394         assertNull(calibrator.getEstimatedSy());
10395         assertNull(calibrator.getEstimatedSz());
10396         assertNull(calibrator.getEstimatedMxy());
10397         assertNull(calibrator.getEstimatedMxz());
10398         assertNull(calibrator.getEstimatedMyx());
10399         assertNull(calibrator.getEstimatedMyz());
10400         assertNull(calibrator.getEstimatedMzx());
10401         assertNull(calibrator.getEstimatedMzy());
10402         assertNull(calibrator.getEstimatedCovariance());
10403         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10404         assertNotNull(calibrator.getGroundTruthGravityNorm());
10405         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10406         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10407         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10408                 ABSOLUTE_ERROR));
10409         final var g = new Acceleration(0.0, AccelerationUnit.G);
10410         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10411         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10412         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10413 
10414         // Force IllegalArgumentException
10415         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10416                 -gravityNorm, measurements, bx, by, bz));
10417     }
10418 
10419     @Test
10420     void testConstructor98() throws WrongSizeException {
10421         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10422 
10423         final var ba = generateBa();
10424         final var biasX = ba.getElementAtIndex(0);
10425         final var biasY = ba.getElementAtIndex(1);
10426         final var biasZ = ba.getElementAtIndex(2);
10427 
10428         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10429         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10430         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10431 
10432         final var randomizer = new UniformRandomizer();
10433         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10434         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10435         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10436         final var nedPosition = new NEDPosition(latitude, longitude, height);
10437         final var nedVelocity = new NEDVelocity();
10438         final var ecefPosition = new ECEFPosition();
10439         final var ecefVelocity = new ECEFVelocity();
10440         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10441         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10442                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10443         final var gravityNorm = gravity.getNorm();
10444 
10445         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
10446                 this);
10447 
10448         // check default values
10449         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10450         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10451         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10452         final var bx1 = calibrator.getBiasXAsAcceleration();
10453         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10454         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10455         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10456         calibrator.getBiasXAsAcceleration(bx2);
10457         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10458         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10459         final var by1 = calibrator.getBiasYAsAcceleration();
10460         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10461         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10462         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10463         calibrator.getBiasYAsAcceleration(by2);
10464         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10465         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10466         final var bz1 = calibrator.getBiasZAsAcceleration();
10467         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10468         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10469         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10470         calibrator.getBiasZAsAcceleration(bz2);
10471         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10472         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10473         final var biasTriad1 = calibrator.getBiasAsTriad();
10474         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10475         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10476         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10477         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10478         final var biasTriad2 = new AccelerationTriad();
10479         calibrator.getBiasAsTriad(biasTriad2);
10480         assertEquals(biasTriad1, biasTriad2);
10481         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10482         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10483         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10484         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10485         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10486         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10487         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10488         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10489         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10490         final var bias1 = calibrator.getBias();
10491         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10492         final var bias2 = new double[3];
10493         calibrator.getBias(bias2);
10494         assertArrayEquals(bias1, bias2, 0.0);
10495         final var b1 = calibrator.getBiasAsMatrix();
10496         assertEquals(b1, ba);
10497         final var b2 = new Matrix(3, 1);
10498         calibrator.getBiasAsMatrix(b2);
10499         assertEquals(b1, b2);
10500         final var ma1 = calibrator.getInitialMa();
10501         assertEquals(new Matrix(3, 3), ma1);
10502         final var ma2 = new Matrix(3, 3);
10503         calibrator.getInitialMa(ma2);
10504         assertEquals(ma1, ma2);
10505         assertSame(measurements, calibrator.getMeasurements());
10506         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10507                 calibrator.getMeasurementType());
10508         assertFalse(calibrator.isOrderedMeasurementsRequired());
10509         assertFalse(calibrator.isQualityScoresRequired());
10510         assertFalse(calibrator.isCommonAxisUsed());
10511         assertSame(this, calibrator.getListener());
10512         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
10513         assertFalse(calibrator.isReady());
10514         assertFalse(calibrator.isRunning());
10515         assertNull(calibrator.getEstimatedMa());
10516         assertNull(calibrator.getEstimatedSx());
10517         assertNull(calibrator.getEstimatedSy());
10518         assertNull(calibrator.getEstimatedSz());
10519         assertNull(calibrator.getEstimatedMxy());
10520         assertNull(calibrator.getEstimatedMxz());
10521         assertNull(calibrator.getEstimatedMyx());
10522         assertNull(calibrator.getEstimatedMyz());
10523         assertNull(calibrator.getEstimatedMzx());
10524         assertNull(calibrator.getEstimatedMzy());
10525         assertNull(calibrator.getEstimatedCovariance());
10526         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10527         assertNotNull(calibrator.getGroundTruthGravityNorm());
10528         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10529         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10530         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10531                 ABSOLUTE_ERROR));
10532         final var g = new Acceleration(0.0, AccelerationUnit.G);
10533         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10534         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10535         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10536 
10537         // Force IllegalArgumentException
10538         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10539                 -gravityNorm, measurements, bx, by, bz, this));
10540     }
10541 
10542     @Test
10543     void testConstructor99() throws WrongSizeException {
10544         final var ba = generateBa();
10545         final var biasX = ba.getElementAtIndex(0);
10546         final var biasY = ba.getElementAtIndex(1);
10547         final var biasZ = ba.getElementAtIndex(2);
10548 
10549         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10550         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10551         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10552 
10553         final var randomizer = new UniformRandomizer();
10554         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10555         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10556         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10557         final var nedPosition = new NEDPosition(latitude, longitude, height);
10558         final var nedVelocity = new NEDVelocity();
10559         final var ecefPosition = new ECEFPosition();
10560         final var ecefVelocity = new ECEFVelocity();
10561         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10562         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10563                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10564         final var gravityNorm = gravity.getNorm();
10565 
10566         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
10567                 bx, by, bz);
10568 
10569         // check default values
10570         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10571         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10572         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10573         final var bx1 = calibrator.getBiasXAsAcceleration();
10574         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10575         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10576         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10577         calibrator.getBiasXAsAcceleration(bx2);
10578         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10579         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10580         final var by1 = calibrator.getBiasYAsAcceleration();
10581         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10582         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10583         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10584         calibrator.getBiasYAsAcceleration(by2);
10585         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10586         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10587         final var bz1 = calibrator.getBiasZAsAcceleration();
10588         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10589         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10590         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10591         calibrator.getBiasZAsAcceleration(bz2);
10592         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10593         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10594         final var biasTriad1 = calibrator.getBiasAsTriad();
10595         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10596         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10597         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10598         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10599         final var biasTriad2 = new AccelerationTriad();
10600         calibrator.getBiasAsTriad(biasTriad2);
10601         assertEquals(biasTriad1, biasTriad2);
10602         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10603         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10604         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10605         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10606         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10607         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10608         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10609         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10610         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10611         final var bias1 = calibrator.getBias();
10612         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10613         final var bias2 = new double[3];
10614         calibrator.getBias(bias2);
10615         assertArrayEquals(bias1, bias2, 0.0);
10616         final var b1 = calibrator.getBiasAsMatrix();
10617         assertEquals(b1, ba);
10618         final var b2 = new Matrix(3, 1);
10619         calibrator.getBiasAsMatrix(b2);
10620         assertEquals(b1, b2);
10621         final var ma1 = calibrator.getInitialMa();
10622         assertEquals(new Matrix(3, 3), ma1);
10623         final var ma2 = new Matrix(3, 3);
10624         calibrator.getInitialMa(ma2);
10625         assertEquals(ma1, ma2);
10626         assertNull(calibrator.getMeasurements());
10627         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10628                 calibrator.getMeasurementType());
10629         assertFalse(calibrator.isOrderedMeasurementsRequired());
10630         assertFalse(calibrator.isQualityScoresRequired());
10631         assertTrue(calibrator.isCommonAxisUsed());
10632         assertNull(calibrator.getListener());
10633         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10634         assertFalse(calibrator.isReady());
10635         assertFalse(calibrator.isRunning());
10636         assertNull(calibrator.getEstimatedMa());
10637         assertNull(calibrator.getEstimatedSx());
10638         assertNull(calibrator.getEstimatedSy());
10639         assertNull(calibrator.getEstimatedSz());
10640         assertNull(calibrator.getEstimatedMxy());
10641         assertNull(calibrator.getEstimatedMxz());
10642         assertNull(calibrator.getEstimatedMyx());
10643         assertNull(calibrator.getEstimatedMyz());
10644         assertNull(calibrator.getEstimatedMzx());
10645         assertNull(calibrator.getEstimatedMzy());
10646         assertNull(calibrator.getEstimatedCovariance());
10647         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10648         assertNotNull(calibrator.getGroundTruthGravityNorm());
10649         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10650         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10651         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10652                 ABSOLUTE_ERROR));
10653         final var g = new Acceleration(0.0, AccelerationUnit.G);
10654         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10655         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10656         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10657 
10658         // Force IllegalArgumentException
10659         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10660                 -gravityNorm, true, bx, by, bz));
10661     }
10662 
10663     @Test
10664     void testConstructor100() throws WrongSizeException {
10665         final var ba = generateBa();
10666         final var biasX = ba.getElementAtIndex(0);
10667         final var biasY = ba.getElementAtIndex(1);
10668         final var biasZ = ba.getElementAtIndex(2);
10669 
10670         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10671         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10672         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10673 
10674         final var randomizer = new UniformRandomizer();
10675         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10676         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10677         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10678         final var nedPosition = new NEDPosition(latitude, longitude, height);
10679         final var nedVelocity = new NEDVelocity();
10680         final var ecefPosition = new ECEFPosition();
10681         final var ecefVelocity = new ECEFVelocity();
10682         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10683         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10684                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10685         final var gravityNorm = gravity.getNorm();
10686 
10687         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
10688                 bx, by, bz, this);
10689 
10690         // check default values
10691         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10692         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10693         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10694         final var bx1 = calibrator.getBiasXAsAcceleration();
10695         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10696         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10697         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10698         calibrator.getBiasXAsAcceleration(bx2);
10699         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10700         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10701         final var by1 = calibrator.getBiasYAsAcceleration();
10702         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10703         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10704         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10705         calibrator.getBiasYAsAcceleration(by2);
10706         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10707         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10708         final var bz1 = calibrator.getBiasZAsAcceleration();
10709         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10710         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10711         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10712         calibrator.getBiasZAsAcceleration(bz2);
10713         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10714         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10715         final var biasTriad1 = calibrator.getBiasAsTriad();
10716         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10717         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10718         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10719         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10720         final var biasTriad2 = new AccelerationTriad();
10721         calibrator.getBiasAsTriad(biasTriad2);
10722         assertEquals(biasTriad1, biasTriad2);
10723         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10724         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10725         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10726         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10727         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10728         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10729         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10730         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10731         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10732         final var bias1 = calibrator.getBias();
10733         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10734         final var bias2 = new double[3];
10735         calibrator.getBias(bias2);
10736         assertArrayEquals(bias1, bias2, 0.0);
10737         final var b1 = calibrator.getBiasAsMatrix();
10738         assertEquals(b1, ba);
10739         final var b2 = new Matrix(3, 1);
10740         calibrator.getBiasAsMatrix(b2);
10741         assertEquals(b1, b2);
10742         final var ma1 = calibrator.getInitialMa();
10743         assertEquals(new Matrix(3, 3), ma1);
10744         final var ma2 = new Matrix(3, 3);
10745         calibrator.getInitialMa(ma2);
10746         assertEquals(ma1, ma2);
10747         assertNull(calibrator.getMeasurements());
10748         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10749                 calibrator.getMeasurementType());
10750         assertFalse(calibrator.isOrderedMeasurementsRequired());
10751         assertFalse(calibrator.isQualityScoresRequired());
10752         assertTrue(calibrator.isCommonAxisUsed());
10753         assertSame(this, calibrator.getListener());
10754         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10755         assertFalse(calibrator.isReady());
10756         assertFalse(calibrator.isRunning());
10757         assertNull(calibrator.getEstimatedMa());
10758         assertNull(calibrator.getEstimatedSx());
10759         assertNull(calibrator.getEstimatedSy());
10760         assertNull(calibrator.getEstimatedSz());
10761         assertNull(calibrator.getEstimatedMxy());
10762         assertNull(calibrator.getEstimatedMxz());
10763         assertNull(calibrator.getEstimatedMyx());
10764         assertNull(calibrator.getEstimatedMyz());
10765         assertNull(calibrator.getEstimatedMzx());
10766         assertNull(calibrator.getEstimatedMzy());
10767         assertNull(calibrator.getEstimatedCovariance());
10768         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10769         assertNotNull(calibrator.getGroundTruthGravityNorm());
10770         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10771         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10772         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10773                 ABSOLUTE_ERROR));
10774         final var g = new Acceleration(0.0, AccelerationUnit.G);
10775         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10776         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10777         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10778 
10779         // Force IllegalArgumentException
10780         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10781                 -gravityNorm, true, bx, by, bz, this));
10782     }
10783 
10784     @Test
10785     void testConstructor101() throws WrongSizeException {
10786         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10787 
10788         final var ba = generateBa();
10789         final var biasX = ba.getElementAtIndex(0);
10790         final var biasY = ba.getElementAtIndex(1);
10791         final var biasZ = ba.getElementAtIndex(2);
10792 
10793         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10794         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10795         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10796 
10797         final var randomizer = new UniformRandomizer();
10798         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10799         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10800         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10801         final var nedPosition = new NEDPosition(latitude, longitude, height);
10802         final var nedVelocity = new NEDVelocity();
10803         final var ecefPosition = new ECEFPosition();
10804         final var ecefVelocity = new ECEFVelocity();
10805         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10806         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10807                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10808         final var gravityNorm = gravity.getNorm();
10809 
10810         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
10811                 true, bx, by, bz);
10812 
10813         // check default values
10814         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10815         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10816         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10817         final var bx1 = calibrator.getBiasXAsAcceleration();
10818         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10819         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10820         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10821         calibrator.getBiasXAsAcceleration(bx2);
10822         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10823         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10824         final var by1 = calibrator.getBiasYAsAcceleration();
10825         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10826         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10827         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10828         calibrator.getBiasYAsAcceleration(by2);
10829         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10830         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10831         final var bz1 = calibrator.getBiasZAsAcceleration();
10832         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10833         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10834         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10835         calibrator.getBiasZAsAcceleration(bz2);
10836         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10837         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10838         final var biasTriad1 = calibrator.getBiasAsTriad();
10839         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10840         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10841         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10842         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10843         final var biasTriad2 = new AccelerationTriad();
10844         calibrator.getBiasAsTriad(biasTriad2);
10845         assertEquals(biasTriad1, biasTriad2);
10846         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10847         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10848         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10849         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10850         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10851         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10852         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10853         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10854         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10855         final var bias1 = calibrator.getBias();
10856         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10857         final var bias2 = new double[3];
10858         calibrator.getBias(bias2);
10859         assertArrayEquals(bias1, bias2, 0.0);
10860         final var b1 = calibrator.getBiasAsMatrix();
10861         assertEquals(b1, ba);
10862         final var b2 = new Matrix(3, 1);
10863         calibrator.getBiasAsMatrix(b2);
10864         assertEquals(b1, b2);
10865         final var ma1 = calibrator.getInitialMa();
10866         assertEquals(new Matrix(3, 3), ma1);
10867         final var ma2 = new Matrix(3, 3);
10868         calibrator.getInitialMa(ma2);
10869         assertEquals(ma1, ma2);
10870         assertSame(measurements, calibrator.getMeasurements());
10871         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10872                 calibrator.getMeasurementType());
10873         assertFalse(calibrator.isOrderedMeasurementsRequired());
10874         assertFalse(calibrator.isQualityScoresRequired());
10875         assertTrue(calibrator.isCommonAxisUsed());
10876         assertNull(calibrator.getListener());
10877         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
10878         assertFalse(calibrator.isReady());
10879         assertFalse(calibrator.isRunning());
10880         assertNull(calibrator.getEstimatedMa());
10881         assertNull(calibrator.getEstimatedSx());
10882         assertNull(calibrator.getEstimatedSy());
10883         assertNull(calibrator.getEstimatedSz());
10884         assertNull(calibrator.getEstimatedMxy());
10885         assertNull(calibrator.getEstimatedMxz());
10886         assertNull(calibrator.getEstimatedMyx());
10887         assertNull(calibrator.getEstimatedMyz());
10888         assertNull(calibrator.getEstimatedMzx());
10889         assertNull(calibrator.getEstimatedMzy());
10890         assertNull(calibrator.getEstimatedCovariance());
10891         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
10892         assertNotNull(calibrator.getGroundTruthGravityNorm());
10893         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10894         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10895         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
10896                 ABSOLUTE_ERROR));
10897         final var g = new Acceleration(0.0, AccelerationUnit.G);
10898         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10899         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10900         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
10901 
10902         // Force IllegalArgumentException
10903         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
10904                 -gravityNorm, measurements, true, bx, by, bz));
10905     }
10906 
10907     @Test
10908     void testConstructor102() throws WrongSizeException {
10909         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
10910 
10911         final var ba = generateBa();
10912         final var biasX = ba.getElementAtIndex(0);
10913         final var biasY = ba.getElementAtIndex(1);
10914         final var biasZ = ba.getElementAtIndex(2);
10915 
10916         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10917         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10918         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
10919 
10920         final var randomizer = new UniformRandomizer();
10921         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
10922         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10923         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10924         final var nedPosition = new NEDPosition(latitude, longitude, height);
10925         final var nedVelocity = new NEDVelocity();
10926         final var ecefPosition = new ECEFPosition();
10927         final var ecefVelocity = new ECEFVelocity();
10928         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
10929         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10930                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10931         final var gravityNorm = gravity.getNorm();
10932 
10933         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
10934                 true, bx, by, bz, this);
10935 
10936         // check default values
10937         assertEquals(biasX, calibrator.getBiasX(), 0.0);
10938         assertEquals(biasY, calibrator.getBiasY(), 0.0);
10939         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
10940         final var bx1 = calibrator.getBiasXAsAcceleration();
10941         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
10942         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
10943         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10944         calibrator.getBiasXAsAcceleration(bx2);
10945         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
10946         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
10947         final var by1 = calibrator.getBiasYAsAcceleration();
10948         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
10949         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
10950         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10951         calibrator.getBiasYAsAcceleration(by2);
10952         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
10953         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
10954         final var bz1 = calibrator.getBiasZAsAcceleration();
10955         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
10956         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
10957         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
10958         calibrator.getBiasZAsAcceleration(bz2);
10959         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
10960         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
10961         final var biasTriad1 = calibrator.getBiasAsTriad();
10962         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
10963         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
10964         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
10965         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
10966         final var biasTriad2 = new AccelerationTriad();
10967         calibrator.getBiasAsTriad(biasTriad2);
10968         assertEquals(biasTriad1, biasTriad2);
10969         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
10970         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
10971         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
10972         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
10973         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
10974         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
10975         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
10976         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
10977         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
10978         final var bias1 = calibrator.getBias();
10979         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10980         final var bias2 = new double[3];
10981         calibrator.getBias(bias2);
10982         assertArrayEquals(bias1, bias2, 0.0);
10983         final var b1 = calibrator.getBiasAsMatrix();
10984         assertEquals(b1, ba);
10985         final var b2 = new Matrix(3, 1);
10986         calibrator.getBiasAsMatrix(b2);
10987         assertEquals(b1, b2);
10988         final var ma1 = calibrator.getInitialMa();
10989         assertEquals(new Matrix(3, 3), ma1);
10990         final var ma2 = new Matrix(3, 3);
10991         calibrator.getInitialMa(ma2);
10992         assertEquals(ma1, ma2);
10993         assertSame(measurements, calibrator.getMeasurements());
10994         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
10995                 calibrator.getMeasurementType());
10996         assertFalse(calibrator.isOrderedMeasurementsRequired());
10997         assertFalse(calibrator.isQualityScoresRequired());
10998         assertTrue(calibrator.isCommonAxisUsed());
10999         assertSame(this, calibrator.getListener());
11000         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11001         assertFalse(calibrator.isReady());
11002         assertFalse(calibrator.isRunning());
11003         assertNull(calibrator.getEstimatedMa());
11004         assertNull(calibrator.getEstimatedSx());
11005         assertNull(calibrator.getEstimatedSy());
11006         assertNull(calibrator.getEstimatedSz());
11007         assertNull(calibrator.getEstimatedMxy());
11008         assertNull(calibrator.getEstimatedMxz());
11009         assertNull(calibrator.getEstimatedMyx());
11010         assertNull(calibrator.getEstimatedMyz());
11011         assertNull(calibrator.getEstimatedMzx());
11012         assertNull(calibrator.getEstimatedMzy());
11013         assertNull(calibrator.getEstimatedCovariance());
11014         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11015         assertNotNull(calibrator.getGroundTruthGravityNorm());
11016         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11017         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11018         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11019                 ABSOLUTE_ERROR));
11020         final var g = new Acceleration(0.0, AccelerationUnit.G);
11021         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11022         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11023         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11024 
11025         // Force IllegalArgumentException
11026         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11027                 -gravityNorm, measurements, true, bx, by, bz, this));
11028     }
11029 
11030     @Test
11031     void testConstructor103() throws WrongSizeException {
11032         final var ba = generateBa();
11033         final var biasX = ba.getElementAtIndex(0);
11034         final var biasY = ba.getElementAtIndex(1);
11035         final var biasZ = ba.getElementAtIndex(2);
11036 
11037         final var ma = generateMaCommonAxis();
11038         final var sx = ma.getElementAt(0, 0);
11039         final var sy = ma.getElementAt(1, 1);
11040         final var sz = ma.getElementAt(2, 2);
11041 
11042         final var randomizer = new UniformRandomizer();
11043         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11044         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11045         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11046         final var nedPosition = new NEDPosition(latitude, longitude, height);
11047         final var nedVelocity = new NEDVelocity();
11048         final var ecefPosition = new ECEFPosition();
11049         final var ecefVelocity = new ECEFVelocity();
11050         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11051         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11052                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11053         final var gravityNorm = gravity.getNorm();
11054 
11055         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
11056                 sx, sy, sz);
11057 
11058         // check default values
11059         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11060         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11061         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11062         final var bx1 = calibrator.getBiasXAsAcceleration();
11063         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11064         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11065         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11066         calibrator.getBiasXAsAcceleration(bx2);
11067         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11068         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11069         final var by1 = calibrator.getBiasYAsAcceleration();
11070         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11071         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11072         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11073         calibrator.getBiasYAsAcceleration(by2);
11074         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11075         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11076         final var bz1 = calibrator.getBiasZAsAcceleration();
11077         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11078         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11079         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11080         calibrator.getBiasZAsAcceleration(bz2);
11081         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11082         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11083         final var biasTriad1 = calibrator.getBiasAsTriad();
11084         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11085         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11086         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11087         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11088         final var biasTriad2 = new AccelerationTriad();
11089         calibrator.getBiasAsTriad(biasTriad2);
11090         assertEquals(biasTriad1, biasTriad2);
11091         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11092         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11093         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11094         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11095         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11096         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11097         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11098         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11099         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11100         final var bias1 = calibrator.getBias();
11101         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11102         final var bias2 = new double[3];
11103         calibrator.getBias(bias2);
11104         assertArrayEquals(bias1, bias2, 0.0);
11105         final var b1 = calibrator.getBiasAsMatrix();
11106         assertEquals(b1, ba);
11107         final var b2 = new Matrix(3, 1);
11108         calibrator.getBiasAsMatrix(b2);
11109         assertEquals(b1, b2);
11110         final var ma1 = calibrator.getInitialMa();
11111         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11112         final var ma2 = new Matrix(3, 3);
11113         calibrator.getInitialMa(ma2);
11114         assertEquals(ma1, ma2);
11115         assertNull(calibrator.getMeasurements());
11116         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11117                 calibrator.getMeasurementType());
11118         assertFalse(calibrator.isOrderedMeasurementsRequired());
11119         assertFalse(calibrator.isQualityScoresRequired());
11120         assertFalse(calibrator.isCommonAxisUsed());
11121         assertNull(calibrator.getListener());
11122         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11123         assertFalse(calibrator.isReady());
11124         assertFalse(calibrator.isRunning());
11125         assertNull(calibrator.getEstimatedMa());
11126         assertNull(calibrator.getEstimatedSx());
11127         assertNull(calibrator.getEstimatedSy());
11128         assertNull(calibrator.getEstimatedSz());
11129         assertNull(calibrator.getEstimatedMxy());
11130         assertNull(calibrator.getEstimatedMxz());
11131         assertNull(calibrator.getEstimatedMyx());
11132         assertNull(calibrator.getEstimatedMyz());
11133         assertNull(calibrator.getEstimatedMzx());
11134         assertNull(calibrator.getEstimatedMzy());
11135         assertNull(calibrator.getEstimatedCovariance());
11136         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11137         assertNotNull(calibrator.getGroundTruthGravityNorm());
11138         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11139         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11140         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11141                 ABSOLUTE_ERROR));
11142         final var g = new Acceleration(0.0, AccelerationUnit.G);
11143         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11144         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11145         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11146 
11147         // Force IllegalArgumentException
11148         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11149                 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz));
11150     }
11151 
11152     @Test
11153     void testConstructor104() throws WrongSizeException {
11154         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11155 
11156         final var ba = generateBa();
11157         final var biasX = ba.getElementAtIndex(0);
11158         final var biasY = ba.getElementAtIndex(1);
11159         final var biasZ = ba.getElementAtIndex(2);
11160 
11161         final var ma = generateMaCommonAxis();
11162         final var sx = ma.getElementAt(0, 0);
11163         final var sy = ma.getElementAt(1, 1);
11164         final var sz = ma.getElementAt(2, 2);
11165 
11166         final var randomizer = new UniformRandomizer();
11167         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11168         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11169         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11170         final var nedPosition = new NEDPosition(latitude, longitude, height);
11171         final var nedVelocity = new NEDVelocity();
11172         final var ecefPosition = new ECEFPosition();
11173         final var ecefVelocity = new ECEFVelocity();
11174         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11175         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11176                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11177         final var gravityNorm = gravity.getNorm();
11178 
11179         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11180                 biasX, biasY, biasZ, sx, sy, sz);
11181 
11182         // check default values
11183         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11184         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11185         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11186         final var bx1 = calibrator.getBiasXAsAcceleration();
11187         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11188         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11189         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11190         calibrator.getBiasXAsAcceleration(bx2);
11191         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11192         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11193         final var by1 = calibrator.getBiasYAsAcceleration();
11194         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11195         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11196         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11197         calibrator.getBiasYAsAcceleration(by2);
11198         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11199         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11200         final var bz1 = calibrator.getBiasZAsAcceleration();
11201         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11202         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11203         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11204         calibrator.getBiasZAsAcceleration(bz2);
11205         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11206         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11207         final var biasTriad1 = calibrator.getBiasAsTriad();
11208         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11209         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11210         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11211         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11212         final var biasTriad2 = new AccelerationTriad();
11213         calibrator.getBiasAsTriad(biasTriad2);
11214         assertEquals(biasTriad1, biasTriad2);
11215         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11216         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11217         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11218         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11219         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11220         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11221         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11222         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11223         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11224         final var bias1 = calibrator.getBias();
11225         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11226         final var bias2 = new double[3];
11227         calibrator.getBias(bias2);
11228         assertArrayEquals(bias1, bias2, 0.0);
11229         final var b1 = calibrator.getBiasAsMatrix();
11230         assertEquals(b1, ba);
11231         final var b2 = new Matrix(3, 1);
11232         calibrator.getBiasAsMatrix(b2);
11233         assertEquals(b1, b2);
11234         final var ma1 = calibrator.getInitialMa();
11235         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11236         final var ma2 = new Matrix(3, 3);
11237         calibrator.getInitialMa(ma2);
11238         assertEquals(ma1, ma2);
11239         assertSame(measurements, calibrator.getMeasurements());
11240         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11241                 calibrator.getMeasurementType());
11242         assertFalse(calibrator.isOrderedMeasurementsRequired());
11243         assertFalse(calibrator.isQualityScoresRequired());
11244         assertFalse(calibrator.isCommonAxisUsed());
11245         assertNull(calibrator.getListener());
11246         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11247         assertFalse(calibrator.isReady());
11248         assertFalse(calibrator.isRunning());
11249         assertNull(calibrator.getEstimatedMa());
11250         assertNull(calibrator.getEstimatedSx());
11251         assertNull(calibrator.getEstimatedSy());
11252         assertNull(calibrator.getEstimatedSz());
11253         assertNull(calibrator.getEstimatedMxy());
11254         assertNull(calibrator.getEstimatedMxz());
11255         assertNull(calibrator.getEstimatedMyx());
11256         assertNull(calibrator.getEstimatedMyz());
11257         assertNull(calibrator.getEstimatedMzx());
11258         assertNull(calibrator.getEstimatedMzy());
11259         assertNull(calibrator.getEstimatedCovariance());
11260         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11261         assertNotNull(calibrator.getGroundTruthGravityNorm());
11262         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11263         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11264         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11265                 ABSOLUTE_ERROR));
11266         final var g = new Acceleration(0.0, AccelerationUnit.G);
11267         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11268         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11269         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11270 
11271         // Force IllegalArgumentException
11272         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11273                 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz));
11274     }
11275 
11276     @Test
11277     void testConstructor105() throws WrongSizeException {
11278         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11279 
11280         final var ba = generateBa();
11281         final var biasX = ba.getElementAtIndex(0);
11282         final var biasY = ba.getElementAtIndex(1);
11283         final var biasZ = ba.getElementAtIndex(2);
11284 
11285         final var ma = generateMaCommonAxis();
11286         final var sx = ma.getElementAt(0, 0);
11287         final var sy = ma.getElementAt(1, 1);
11288         final var sz = ma.getElementAt(2, 2);
11289 
11290         final var randomizer = new UniformRandomizer();
11291         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11292         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11293         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11294         final var nedPosition = new NEDPosition(latitude, longitude, height);
11295         final var nedVelocity = new NEDVelocity();
11296         final var ecefPosition = new ECEFPosition();
11297         final var ecefVelocity = new ECEFVelocity();
11298         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11299         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11300                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11301         final var gravityNorm = gravity.getNorm();
11302 
11303         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11304                 biasX, biasY, biasZ, sx, sy, sz, this);
11305 
11306         // check default values
11307         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11308         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11309         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11310         final var bx1 = calibrator.getBiasXAsAcceleration();
11311         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11312         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11313         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11314         calibrator.getBiasXAsAcceleration(bx2);
11315         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11316         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11317         final var by1 = calibrator.getBiasYAsAcceleration();
11318         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11319         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11320         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11321         calibrator.getBiasYAsAcceleration(by2);
11322         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11323         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11324         final var bz1 = calibrator.getBiasZAsAcceleration();
11325         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11326         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11327         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11328         calibrator.getBiasZAsAcceleration(bz2);
11329         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11330         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11331         final var biasTriad1 = calibrator.getBiasAsTriad();
11332         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11333         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11334         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11335         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11336         final var biasTriad2 = new AccelerationTriad();
11337         calibrator.getBiasAsTriad(biasTriad2);
11338         assertEquals(biasTriad1, biasTriad2);
11339         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11340         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11341         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11342         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11343         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11344         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11345         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11346         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11347         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11348         final var bias1 = calibrator.getBias();
11349         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11350         final var bias2 = new double[3];
11351         calibrator.getBias(bias2);
11352         assertArrayEquals(bias1, bias2, 0.0);
11353         final var b1 = calibrator.getBiasAsMatrix();
11354         assertEquals(b1, ba);
11355         final var b2 = new Matrix(3, 1);
11356         calibrator.getBiasAsMatrix(b2);
11357         assertEquals(b1, b2);
11358         final var ma1 = calibrator.getInitialMa();
11359         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11360         final var ma2 = new Matrix(3, 3);
11361         calibrator.getInitialMa(ma2);
11362         assertEquals(ma1, ma2);
11363         assertSame(measurements, calibrator.getMeasurements());
11364         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11365                 calibrator.getMeasurementType());
11366         assertFalse(calibrator.isOrderedMeasurementsRequired());
11367         assertFalse(calibrator.isQualityScoresRequired());
11368         assertFalse(calibrator.isCommonAxisUsed());
11369         assertSame(this, calibrator.getListener());
11370         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11371         assertFalse(calibrator.isReady());
11372         assertFalse(calibrator.isRunning());
11373         assertNull(calibrator.getEstimatedMa());
11374         assertNull(calibrator.getEstimatedSx());
11375         assertNull(calibrator.getEstimatedSy());
11376         assertNull(calibrator.getEstimatedSz());
11377         assertNull(calibrator.getEstimatedMxy());
11378         assertNull(calibrator.getEstimatedMxz());
11379         assertNull(calibrator.getEstimatedMyx());
11380         assertNull(calibrator.getEstimatedMyz());
11381         assertNull(calibrator.getEstimatedMzx());
11382         assertNull(calibrator.getEstimatedMzy());
11383         assertNull(calibrator.getEstimatedCovariance());
11384         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11385         assertNotNull(calibrator.getGroundTruthGravityNorm());
11386         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11387         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11388         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11389                 ABSOLUTE_ERROR));
11390         final var g = new Acceleration(0.0, AccelerationUnit.G);
11391         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11392         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11393         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11394 
11395         // Force IllegalArgumentException
11396         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11397                 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, this));
11398     }
11399 
11400     @Test
11401     void testConstructor106() throws WrongSizeException {
11402         final var ba = generateBa();
11403         final var biasX = ba.getElementAtIndex(0);
11404         final var biasY = ba.getElementAtIndex(1);
11405         final var biasZ = ba.getElementAtIndex(2);
11406 
11407         final var ma = generateMaCommonAxis();
11408         final var sx = ma.getElementAt(0, 0);
11409         final var sy = ma.getElementAt(1, 1);
11410         final var sz = ma.getElementAt(2, 2);
11411 
11412         final var randomizer = new UniformRandomizer();
11413         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11414         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11415         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11416         final var nedPosition = new NEDPosition(latitude, longitude, height);
11417         final var nedVelocity = new NEDVelocity();
11418         final var ecefPosition = new ECEFPosition();
11419         final var ecefVelocity = new ECEFVelocity();
11420         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11421         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11422                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11423         final var gravityNorm = gravity.getNorm();
11424 
11425         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
11426                 biasX, biasY, biasZ, sx, sy, sz);
11427 
11428         // check default values
11429         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11430         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11431         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11432         final var bx1 = calibrator.getBiasXAsAcceleration();
11433         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11434         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11435         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11436         calibrator.getBiasXAsAcceleration(bx2);
11437         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11438         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11439         final var by1 = calibrator.getBiasYAsAcceleration();
11440         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11441         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11442         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11443         calibrator.getBiasYAsAcceleration(by2);
11444         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11445         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11446         final var bz1 = calibrator.getBiasZAsAcceleration();
11447         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11448         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11449         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11450         calibrator.getBiasZAsAcceleration(bz2);
11451         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11452         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11453         final var biasTriad1 = calibrator.getBiasAsTriad();
11454         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11455         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11456         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11457         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11458         final var biasTriad2 = new AccelerationTriad();
11459         calibrator.getBiasAsTriad(biasTriad2);
11460         assertEquals(biasTriad1, biasTriad2);
11461         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11462         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11463         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11464         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11465         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11466         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11467         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11468         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11469         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11470         final var bias1 = calibrator.getBias();
11471         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11472         final var bias2 = new double[3];
11473         calibrator.getBias(bias2);
11474         assertArrayEquals(bias1, bias2, 0.0);
11475         final var b1 = calibrator.getBiasAsMatrix();
11476         assertEquals(b1, ba);
11477         final var b2 = new Matrix(3, 1);
11478         calibrator.getBiasAsMatrix(b2);
11479         assertEquals(b1, b2);
11480         final var ma1 = calibrator.getInitialMa();
11481         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11482         final var ma2 = new Matrix(3, 3);
11483         calibrator.getInitialMa(ma2);
11484         assertEquals(ma1, ma2);
11485         assertNull(calibrator.getMeasurements());
11486         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11487                 calibrator.getMeasurementType());
11488         assertFalse(calibrator.isOrderedMeasurementsRequired());
11489         assertFalse(calibrator.isQualityScoresRequired());
11490         assertTrue(calibrator.isCommonAxisUsed());
11491         assertNull(calibrator.getListener());
11492         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11493         assertFalse(calibrator.isReady());
11494         assertFalse(calibrator.isRunning());
11495         assertNull(calibrator.getEstimatedMa());
11496         assertNull(calibrator.getEstimatedSx());
11497         assertNull(calibrator.getEstimatedSy());
11498         assertNull(calibrator.getEstimatedSz());
11499         assertNull(calibrator.getEstimatedMxy());
11500         assertNull(calibrator.getEstimatedMxz());
11501         assertNull(calibrator.getEstimatedMyx());
11502         assertNull(calibrator.getEstimatedMyz());
11503         assertNull(calibrator.getEstimatedMzx());
11504         assertNull(calibrator.getEstimatedMzy());
11505         assertNull(calibrator.getEstimatedCovariance());
11506         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11507         assertNotNull(calibrator.getGroundTruthGravityNorm());
11508         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11509         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11510         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11511                 ABSOLUTE_ERROR));
11512         final var g = new Acceleration(0.0, AccelerationUnit.G);
11513         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11514         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11515         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11516 
11517         // Force IllegalArgumentException
11518         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11519                 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz));
11520     }
11521 
11522     @Test
11523     void testConstructor107() throws WrongSizeException {
11524         final var ba = generateBa();
11525         final var biasX = ba.getElementAtIndex(0);
11526         final var biasY = ba.getElementAtIndex(1);
11527         final var biasZ = ba.getElementAtIndex(2);
11528 
11529         final var ma = generateMaCommonAxis();
11530         final var sx = ma.getElementAt(0, 0);
11531         final var sy = ma.getElementAt(1, 1);
11532         final var sz = ma.getElementAt(2, 2);
11533 
11534         final var randomizer = new UniformRandomizer();
11535         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11536         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11537         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11538         final var nedPosition = new NEDPosition(latitude, longitude, height);
11539         final var nedVelocity = new NEDVelocity();
11540         final var ecefPosition = new ECEFPosition();
11541         final var ecefVelocity = new ECEFVelocity();
11542         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11543         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11544                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11545         final var gravityNorm = gravity.getNorm();
11546 
11547         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
11548                 biasX, biasY, biasZ, sx, sy, sz, this);
11549 
11550         // check default values
11551         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11552         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11553         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11554         final var bx1 = calibrator.getBiasXAsAcceleration();
11555         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11556         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11557         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11558         calibrator.getBiasXAsAcceleration(bx2);
11559         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11560         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11561         final var by1 = calibrator.getBiasYAsAcceleration();
11562         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11563         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11564         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11565         calibrator.getBiasYAsAcceleration(by2);
11566         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11567         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11568         final var bz1 = calibrator.getBiasZAsAcceleration();
11569         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11570         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11571         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11572         calibrator.getBiasZAsAcceleration(bz2);
11573         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11574         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11575         final var biasTriad1 = calibrator.getBiasAsTriad();
11576         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11577         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11578         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11579         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11580         final var biasTriad2 = new AccelerationTriad();
11581         calibrator.getBiasAsTriad(biasTriad2);
11582         assertEquals(biasTriad1, biasTriad2);
11583         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11584         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11585         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11586         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11587         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11588         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11589         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11590         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11591         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11592         final var bias1 = calibrator.getBias();
11593         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11594         final var bias2 = new double[3];
11595         calibrator.getBias(bias2);
11596         assertArrayEquals(bias1, bias2, 0.0);
11597         final var b1 = calibrator.getBiasAsMatrix();
11598         assertEquals(b1, ba);
11599         final var b2 = new Matrix(3, 1);
11600         calibrator.getBiasAsMatrix(b2);
11601         assertEquals(b1, b2);
11602         final var ma1 = calibrator.getInitialMa();
11603         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11604         final var ma2 = new Matrix(3, 3);
11605         calibrator.getInitialMa(ma2);
11606         assertEquals(ma1, ma2);
11607         assertNull(calibrator.getMeasurements());
11608         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11609                 calibrator.getMeasurementType());
11610         assertFalse(calibrator.isOrderedMeasurementsRequired());
11611         assertFalse(calibrator.isQualityScoresRequired());
11612         assertTrue(calibrator.isCommonAxisUsed());
11613         assertSame(this, calibrator.getListener());
11614         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11615         assertFalse(calibrator.isReady());
11616         assertFalse(calibrator.isRunning());
11617         assertNull(calibrator.getEstimatedMa());
11618         assertNull(calibrator.getEstimatedSx());
11619         assertNull(calibrator.getEstimatedSy());
11620         assertNull(calibrator.getEstimatedSz());
11621         assertNull(calibrator.getEstimatedMxy());
11622         assertNull(calibrator.getEstimatedMxz());
11623         assertNull(calibrator.getEstimatedMyx());
11624         assertNull(calibrator.getEstimatedMyz());
11625         assertNull(calibrator.getEstimatedMzx());
11626         assertNull(calibrator.getEstimatedMzy());
11627         assertNull(calibrator.getEstimatedCovariance());
11628         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11629         assertNotNull(calibrator.getGroundTruthGravityNorm());
11630         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11631         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11632         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11633                 ABSOLUTE_ERROR));
11634         final var g = new Acceleration(0.0, AccelerationUnit.G);
11635         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11636         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11637         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11638 
11639         // Force IllegalArgumentException
11640         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11641                 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, this));
11642     }
11643 
11644     @Test
11645     void testConstructor108() throws WrongSizeException {
11646         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11647 
11648         final var ba = generateBa();
11649         final var biasX = ba.getElementAtIndex(0);
11650         final var biasY = ba.getElementAtIndex(1);
11651         final var biasZ = ba.getElementAtIndex(2);
11652 
11653         final var ma = generateMaCommonAxis();
11654         final var sx = ma.getElementAt(0, 0);
11655         final var sy = ma.getElementAt(1, 1);
11656         final var sz = ma.getElementAt(2, 2);
11657 
11658         final var randomizer = new UniformRandomizer();
11659         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11660         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11661         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11662         final var nedPosition = new NEDPosition(latitude, longitude, height);
11663         final var nedVelocity = new NEDVelocity();
11664         final var ecefPosition = new ECEFPosition();
11665         final var ecefVelocity = new ECEFVelocity();
11666         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11667         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11668                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11669         final var gravityNorm = gravity.getNorm();
11670 
11671         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11672                 true, biasX, biasY, biasZ, sx, sy, sz);
11673 
11674         // check default values
11675         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11676         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11677         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11678         final var bx1 = calibrator.getBiasXAsAcceleration();
11679         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11680         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11681         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11682         calibrator.getBiasXAsAcceleration(bx2);
11683         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11684         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11685         final var by1 = calibrator.getBiasYAsAcceleration();
11686         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11687         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11688         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11689         calibrator.getBiasYAsAcceleration(by2);
11690         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11691         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11692         final var bz1 = calibrator.getBiasZAsAcceleration();
11693         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11694         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11695         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11696         calibrator.getBiasZAsAcceleration(bz2);
11697         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11698         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11699         final var biasTriad1 = calibrator.getBiasAsTriad();
11700         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11701         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11702         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11703         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11704         final var biasTriad2 = new AccelerationTriad();
11705         calibrator.getBiasAsTriad(biasTriad2);
11706         assertEquals(biasTriad1, biasTriad2);
11707         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11708         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11709         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11710         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11711         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11712         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11713         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11714         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11715         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11716         final var bias1 = calibrator.getBias();
11717         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11718         final var bias2 = new double[3];
11719         calibrator.getBias(bias2);
11720         assertArrayEquals(bias1, bias2, 0.0);
11721         final var b1 = calibrator.getBiasAsMatrix();
11722         assertEquals(b1, ba);
11723         final var b2 = new Matrix(3, 1);
11724         calibrator.getBiasAsMatrix(b2);
11725         assertEquals(b1, b2);
11726         final var ma1 = calibrator.getInitialMa();
11727         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11728         final var ma2 = new Matrix(3, 3);
11729         calibrator.getInitialMa(ma2);
11730         assertEquals(ma1, ma2);
11731         assertSame(measurements, calibrator.getMeasurements());
11732         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11733                 calibrator.getMeasurementType());
11734         assertFalse(calibrator.isOrderedMeasurementsRequired());
11735         assertFalse(calibrator.isQualityScoresRequired());
11736         assertTrue(calibrator.isCommonAxisUsed());
11737         assertNull(calibrator.getListener());
11738         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11739         assertFalse(calibrator.isReady());
11740         assertFalse(calibrator.isRunning());
11741         assertNull(calibrator.getEstimatedMa());
11742         assertNull(calibrator.getEstimatedSx());
11743         assertNull(calibrator.getEstimatedSy());
11744         assertNull(calibrator.getEstimatedSz());
11745         assertNull(calibrator.getEstimatedMxy());
11746         assertNull(calibrator.getEstimatedMxz());
11747         assertNull(calibrator.getEstimatedMyx());
11748         assertNull(calibrator.getEstimatedMyz());
11749         assertNull(calibrator.getEstimatedMzx());
11750         assertNull(calibrator.getEstimatedMzy());
11751         assertNull(calibrator.getEstimatedCovariance());
11752         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11753         assertNotNull(calibrator.getGroundTruthGravityNorm());
11754         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11755         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11756         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11757                 ABSOLUTE_ERROR));
11758         final var g = new Acceleration(0.0, AccelerationUnit.G);
11759         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11760         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11761         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11762 
11763         // Force IllegalArgumentException
11764         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11765                 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz));
11766     }
11767 
11768     @Test
11769     void testConstructor109() throws WrongSizeException {
11770         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
11771 
11772         final var ba = generateBa();
11773         final var biasX = ba.getElementAtIndex(0);
11774         final var biasY = ba.getElementAtIndex(1);
11775         final var biasZ = ba.getElementAtIndex(2);
11776 
11777         final var ma = generateMaCommonAxis();
11778         final var sx = ma.getElementAt(0, 0);
11779         final var sy = ma.getElementAt(1, 1);
11780         final var sz = ma.getElementAt(2, 2);
11781 
11782         final var randomizer = new UniformRandomizer();
11783         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11784         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11785         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11786         final var nedPosition = new NEDPosition(latitude, longitude, height);
11787         final var nedVelocity = new NEDVelocity();
11788         final var ecefPosition = new ECEFPosition();
11789         final var ecefVelocity = new ECEFVelocity();
11790         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11791         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11792                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11793         final var gravityNorm = gravity.getNorm();
11794 
11795         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
11796                 true, biasX, biasY, biasZ, sx, sy, sz, this);
11797 
11798         // check default values
11799         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11800         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11801         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11802         final var bx1 = calibrator.getBiasXAsAcceleration();
11803         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11804         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11805         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11806         calibrator.getBiasXAsAcceleration(bx2);
11807         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11808         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11809         final var by1 = calibrator.getBiasYAsAcceleration();
11810         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11811         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11812         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11813         calibrator.getBiasYAsAcceleration(by2);
11814         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11815         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11816         final var bz1 = calibrator.getBiasZAsAcceleration();
11817         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11818         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11819         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11820         calibrator.getBiasZAsAcceleration(bz2);
11821         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11822         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11823         final var biasTriad1 = calibrator.getBiasAsTriad();
11824         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11825         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11826         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11827         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11828         final var biasTriad2 = new AccelerationTriad();
11829         calibrator.getBiasAsTriad(biasTriad2);
11830         assertEquals(biasTriad1, biasTriad2);
11831         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11832         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11833         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11834         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11835         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11836         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11837         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11838         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11839         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11840         final var bias1 = calibrator.getBias();
11841         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11842         final var bias2 = new double[3];
11843         calibrator.getBias(bias2);
11844         assertArrayEquals(bias1, bias2, 0.0);
11845         final var b1 = calibrator.getBiasAsMatrix();
11846         assertEquals(b1, ba);
11847         final var b2 = new Matrix(3, 1);
11848         calibrator.getBiasAsMatrix(b2);
11849         assertEquals(b1, b2);
11850         final var ma1 = calibrator.getInitialMa();
11851         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11852         final var ma2 = new Matrix(3, 3);
11853         calibrator.getInitialMa(ma2);
11854         assertEquals(ma1, ma2);
11855         assertSame(measurements, calibrator.getMeasurements());
11856         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11857                 calibrator.getMeasurementType());
11858         assertFalse(calibrator.isOrderedMeasurementsRequired());
11859         assertFalse(calibrator.isQualityScoresRequired());
11860         assertTrue(calibrator.isCommonAxisUsed());
11861         assertSame(this, calibrator.getListener());
11862         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
11863         assertFalse(calibrator.isReady());
11864         assertFalse(calibrator.isRunning());
11865         assertNull(calibrator.getEstimatedMa());
11866         assertNull(calibrator.getEstimatedSx());
11867         assertNull(calibrator.getEstimatedSy());
11868         assertNull(calibrator.getEstimatedSz());
11869         assertNull(calibrator.getEstimatedMxy());
11870         assertNull(calibrator.getEstimatedMxz());
11871         assertNull(calibrator.getEstimatedMyx());
11872         assertNull(calibrator.getEstimatedMyz());
11873         assertNull(calibrator.getEstimatedMzx());
11874         assertNull(calibrator.getEstimatedMzy());
11875         assertNull(calibrator.getEstimatedCovariance());
11876         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
11877         assertNotNull(calibrator.getGroundTruthGravityNorm());
11878         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11879         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11880         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
11881                 ABSOLUTE_ERROR));
11882         final var g = new Acceleration(0.0, AccelerationUnit.G);
11883         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11884         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11885         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
11886 
11887         // Force IllegalArgumentException
11888         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
11889                 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, this));
11890     }
11891 
11892     @Test
11893     void testConstructor110() throws WrongSizeException {
11894         final var ba = generateBa();
11895         final var biasX = ba.getElementAtIndex(0);
11896         final var biasY = ba.getElementAtIndex(1);
11897         final var biasZ = ba.getElementAtIndex(2);
11898 
11899         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
11900         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
11901         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
11902 
11903         final var ma = generateMaCommonAxis();
11904         final var sx = ma.getElementAt(0, 0);
11905         final var sy = ma.getElementAt(1, 1);
11906         final var sz = ma.getElementAt(2, 2);
11907 
11908         final var randomizer = new UniformRandomizer();
11909         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
11910         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11911         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11912         final var nedPosition = new NEDPosition(latitude, longitude, height);
11913         final var nedVelocity = new NEDVelocity();
11914         final var ecefPosition = new ECEFPosition();
11915         final var ecefVelocity = new ECEFVelocity();
11916         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
11917         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11918                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11919         final var gravityNorm = gravity.getNorm();
11920 
11921         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz);
11922 
11923         // check default values
11924         assertEquals(biasX, calibrator.getBiasX(), 0.0);
11925         assertEquals(biasY, calibrator.getBiasY(), 0.0);
11926         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
11927         final var bx1 = calibrator.getBiasXAsAcceleration();
11928         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
11929         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
11930         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11931         calibrator.getBiasXAsAcceleration(bx2);
11932         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
11933         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
11934         final var by1 = calibrator.getBiasYAsAcceleration();
11935         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
11936         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
11937         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11938         calibrator.getBiasYAsAcceleration(by2);
11939         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
11940         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
11941         final var bz1 = calibrator.getBiasZAsAcceleration();
11942         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
11943         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
11944         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
11945         calibrator.getBiasZAsAcceleration(bz2);
11946         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
11947         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
11948         final var biasTriad1 = calibrator.getBiasAsTriad();
11949         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
11950         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
11951         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
11952         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
11953         final var biasTriad2 = new AccelerationTriad();
11954         calibrator.getBiasAsTriad(biasTriad2);
11955         assertEquals(biasTriad1, biasTriad2);
11956         assertEquals(sx, calibrator.getInitialSx(), 0.0);
11957         assertEquals(sy, calibrator.getInitialSy(), 0.0);
11958         assertEquals(sz, calibrator.getInitialSz(), 0.0);
11959         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
11960         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
11961         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
11962         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
11963         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
11964         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
11965         final var bias1 = calibrator.getBias();
11966         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11967         final var bias2 = new double[3];
11968         calibrator.getBias(bias2);
11969         assertArrayEquals(bias1, bias2, 0.0);
11970         final var b1 = calibrator.getBiasAsMatrix();
11971         assertEquals(b1, ba);
11972         final var b2 = new Matrix(3, 1);
11973         calibrator.getBiasAsMatrix(b2);
11974         assertEquals(b1, b2);
11975         final var ma1 = calibrator.getInitialMa();
11976         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
11977         final var ma2 = new Matrix(3, 3);
11978         calibrator.getInitialMa(ma2);
11979         assertEquals(ma1, ma2);
11980         assertNull(calibrator.getMeasurements());
11981         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
11982                 calibrator.getMeasurementType());
11983         assertFalse(calibrator.isOrderedMeasurementsRequired());
11984         assertFalse(calibrator.isQualityScoresRequired());
11985         assertFalse(calibrator.isCommonAxisUsed());
11986         assertNull(calibrator.getListener());
11987         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
11988         assertFalse(calibrator.isReady());
11989         assertFalse(calibrator.isRunning());
11990         assertNull(calibrator.getEstimatedMa());
11991         assertNull(calibrator.getEstimatedSx());
11992         assertNull(calibrator.getEstimatedSy());
11993         assertNull(calibrator.getEstimatedSz());
11994         assertNull(calibrator.getEstimatedMxy());
11995         assertNull(calibrator.getEstimatedMxz());
11996         assertNull(calibrator.getEstimatedMyx());
11997         assertNull(calibrator.getEstimatedMyz());
11998         assertNull(calibrator.getEstimatedMzx());
11999         assertNull(calibrator.getEstimatedMzy());
12000         assertNull(calibrator.getEstimatedCovariance());
12001         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12002         assertNotNull(calibrator.getGroundTruthGravityNorm());
12003         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12004         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12005         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12006                 ABSOLUTE_ERROR));
12007         final var g = new Acceleration(0.0, AccelerationUnit.G);
12008         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12009         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12010         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12011 
12012         // Force IllegalArgumentException
12013         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12014                 -gravityNorm, bx, by, bz, sx, sy, sz));
12015     }
12016 
12017     @Test
12018     void testConstructor111() throws WrongSizeException {
12019         final var ba = generateBa();
12020         final var biasX = ba.getElementAtIndex(0);
12021         final var biasY = ba.getElementAtIndex(1);
12022         final var biasZ = ba.getElementAtIndex(2);
12023 
12024         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12025         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12026         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12027 
12028         final var ma = generateMaCommonAxis();
12029         final var sx = ma.getElementAt(0, 0);
12030         final var sy = ma.getElementAt(1, 1);
12031         final var sz = ma.getElementAt(2, 2);
12032 
12033         final var randomizer = new UniformRandomizer();
12034         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12035         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12036         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12037         final var nedPosition = new NEDPosition(latitude, longitude, height);
12038         final var nedVelocity = new NEDVelocity();
12039         final var ecefPosition = new ECEFPosition();
12040         final var ecefVelocity = new ECEFVelocity();
12041         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12042         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12043                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12044         final var gravityNorm = gravity.getNorm();
12045 
12046         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
12047                 this);
12048 
12049         // check default values
12050         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12051         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12052         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12053         final var bx1 = calibrator.getBiasXAsAcceleration();
12054         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12055         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12056         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12057         calibrator.getBiasXAsAcceleration(bx2);
12058         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12059         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12060         final var by1 = calibrator.getBiasYAsAcceleration();
12061         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12062         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12063         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12064         calibrator.getBiasYAsAcceleration(by2);
12065         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12066         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12067         final var bz1 = calibrator.getBiasZAsAcceleration();
12068         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12069         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12070         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12071         calibrator.getBiasZAsAcceleration(bz2);
12072         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12073         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12074         final var biasTriad1 = calibrator.getBiasAsTriad();
12075         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12076         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12077         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12078         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12079         final var biasTriad2 = new AccelerationTriad();
12080         calibrator.getBiasAsTriad(biasTriad2);
12081         assertEquals(biasTriad1, biasTriad2);
12082         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12083         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12084         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12085         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12086         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12087         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12088         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12089         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12090         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12091         final var bias1 = calibrator.getBias();
12092         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12093         final var bias2 = new double[3];
12094         calibrator.getBias(bias2);
12095         assertArrayEquals(bias1, bias2, 0.0);
12096         final var b1 = calibrator.getBiasAsMatrix();
12097         assertEquals(b1, ba);
12098         final var b2 = new Matrix(3, 1);
12099         calibrator.getBiasAsMatrix(b2);
12100         assertEquals(b1, b2);
12101         final var ma1 = calibrator.getInitialMa();
12102         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12103         final var ma2 = new Matrix(3, 3);
12104         calibrator.getInitialMa(ma2);
12105         assertEquals(ma1, ma2);
12106         assertNull(calibrator.getMeasurements());
12107         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12108                 calibrator.getMeasurementType());
12109         assertFalse(calibrator.isOrderedMeasurementsRequired());
12110         assertFalse(calibrator.isQualityScoresRequired());
12111         assertFalse(calibrator.isCommonAxisUsed());
12112         assertSame(this, calibrator.getListener());
12113         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
12114         assertFalse(calibrator.isReady());
12115         assertFalse(calibrator.isRunning());
12116         assertNull(calibrator.getEstimatedMa());
12117         assertNull(calibrator.getEstimatedSx());
12118         assertNull(calibrator.getEstimatedSy());
12119         assertNull(calibrator.getEstimatedSz());
12120         assertNull(calibrator.getEstimatedMxy());
12121         assertNull(calibrator.getEstimatedMxz());
12122         assertNull(calibrator.getEstimatedMyx());
12123         assertNull(calibrator.getEstimatedMyz());
12124         assertNull(calibrator.getEstimatedMzx());
12125         assertNull(calibrator.getEstimatedMzy());
12126         assertNull(calibrator.getEstimatedCovariance());
12127         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12128         assertNotNull(calibrator.getGroundTruthGravityNorm());
12129         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12130         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12131         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12132                 ABSOLUTE_ERROR));
12133         final var g = new Acceleration(0.0, AccelerationUnit.G);
12134         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12135         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12136         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12137 
12138         // Force IllegalArgumentException
12139         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12140                 -gravityNorm, bx, by, bz, sx, sy, sz, this));
12141     }
12142 
12143     @Test
12144     void testConstructor112() throws WrongSizeException {
12145         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12146 
12147         final var ba = generateBa();
12148         final var biasX = ba.getElementAtIndex(0);
12149         final var biasY = ba.getElementAtIndex(1);
12150         final var biasZ = ba.getElementAtIndex(2);
12151 
12152         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12153         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12154         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12155 
12156         final var ma = generateMaCommonAxis();
12157         final var sx = ma.getElementAt(0, 0);
12158         final var sy = ma.getElementAt(1, 1);
12159         final var sz = ma.getElementAt(2, 2);
12160 
12161         final var randomizer = new UniformRandomizer();
12162         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12163         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12164         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12165         final var nedPosition = new NEDPosition(latitude, longitude, height);
12166         final var nedVelocity = new NEDVelocity();
12167         final var ecefPosition = new ECEFPosition();
12168         final var ecefVelocity = new ECEFVelocity();
12169         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12170         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12171                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12172         final var gravityNorm = gravity.getNorm();
12173 
12174         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
12175                 sx, sy, sz);
12176 
12177         // check default values
12178         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12179         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12180         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12181         final var bx1 = calibrator.getBiasXAsAcceleration();
12182         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12183         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12184         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12185         calibrator.getBiasXAsAcceleration(bx2);
12186         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12187         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12188         final var by1 = calibrator.getBiasYAsAcceleration();
12189         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12190         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12191         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12192         calibrator.getBiasYAsAcceleration(by2);
12193         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12194         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12195         final var bz1 = calibrator.getBiasZAsAcceleration();
12196         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12197         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12198         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12199         calibrator.getBiasZAsAcceleration(bz2);
12200         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12201         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12202         final var biasTriad1 = calibrator.getBiasAsTriad();
12203         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12204         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12205         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12206         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12207         final var biasTriad2 = new AccelerationTriad();
12208         calibrator.getBiasAsTriad(biasTriad2);
12209         assertEquals(biasTriad1, biasTriad2);
12210         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12211         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12212         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12213         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12214         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12215         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12216         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12217         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12218         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12219         final var bias1 = calibrator.getBias();
12220         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12221         final var bias2 = new double[3];
12222         calibrator.getBias(bias2);
12223         assertArrayEquals(bias1, bias2, 0.0);
12224         final var b1 = calibrator.getBiasAsMatrix();
12225         assertEquals(b1, ba);
12226         final var b2 = new Matrix(3, 1);
12227         calibrator.getBiasAsMatrix(b2);
12228         assertEquals(b1, b2);
12229         final var ma1 = calibrator.getInitialMa();
12230         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12231         final var ma2 = new Matrix(3, 3);
12232         calibrator.getInitialMa(ma2);
12233         assertEquals(ma1, ma2);
12234         assertSame(measurements, calibrator.getMeasurements());
12235         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12236                 calibrator.getMeasurementType());
12237         assertFalse(calibrator.isOrderedMeasurementsRequired());
12238         assertFalse(calibrator.isQualityScoresRequired());
12239         assertFalse(calibrator.isCommonAxisUsed());
12240         assertNull(calibrator.getListener());
12241         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
12242         assertFalse(calibrator.isReady());
12243         assertFalse(calibrator.isRunning());
12244         assertNull(calibrator.getEstimatedMa());
12245         assertNull(calibrator.getEstimatedSx());
12246         assertNull(calibrator.getEstimatedSy());
12247         assertNull(calibrator.getEstimatedSz());
12248         assertNull(calibrator.getEstimatedMxy());
12249         assertNull(calibrator.getEstimatedMxz());
12250         assertNull(calibrator.getEstimatedMyx());
12251         assertNull(calibrator.getEstimatedMyz());
12252         assertNull(calibrator.getEstimatedMzx());
12253         assertNull(calibrator.getEstimatedMzy());
12254         assertNull(calibrator.getEstimatedCovariance());
12255         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12256         assertNotNull(calibrator.getGroundTruthGravityNorm());
12257         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12258         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12259         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12260                 ABSOLUTE_ERROR));
12261         final var g = new Acceleration(0.0, AccelerationUnit.G);
12262         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12263         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12264         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12265 
12266         // Force IllegalArgumentException
12267         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12268                 -gravityNorm, measurements, bx, by, bz, sx, sy, sz));
12269     }
12270 
12271     @Test
12272     void testConstructor113() throws WrongSizeException {
12273         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12274 
12275         final var ba = generateBa();
12276         final var biasX = ba.getElementAtIndex(0);
12277         final var biasY = ba.getElementAtIndex(1);
12278         final var biasZ = ba.getElementAtIndex(2);
12279 
12280         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12281         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12282         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12283 
12284         final var ma = generateMaCommonAxis();
12285         final var sx = ma.getElementAt(0, 0);
12286         final var sy = ma.getElementAt(1, 1);
12287         final var sz = ma.getElementAt(2, 2);
12288 
12289         final var randomizer = new UniformRandomizer();
12290         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12291         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12292         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12293         final var nedPosition = new NEDPosition(latitude, longitude, height);
12294         final var nedVelocity = new NEDVelocity();
12295         final var ecefPosition = new ECEFPosition();
12296         final var ecefVelocity = new ECEFVelocity();
12297         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12298         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12299                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12300         final var gravityNorm = gravity.getNorm();
12301 
12302         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
12303                 sx, sy, sz, this);
12304 
12305         // check default values
12306         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12307         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12308         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12309         final var bx1 = calibrator.getBiasXAsAcceleration();
12310         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12311         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12312         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12313         calibrator.getBiasXAsAcceleration(bx2);
12314         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12315         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12316         final var by1 = calibrator.getBiasYAsAcceleration();
12317         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12318         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12319         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12320         calibrator.getBiasYAsAcceleration(by2);
12321         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12322         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12323         final var bz1 = calibrator.getBiasZAsAcceleration();
12324         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12325         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12326         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12327         calibrator.getBiasZAsAcceleration(bz2);
12328         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12329         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12330         final var biasTriad1 = calibrator.getBiasAsTriad();
12331         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12332         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12333         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12334         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12335         final var biasTriad2 = new AccelerationTriad();
12336         calibrator.getBiasAsTriad(biasTriad2);
12337         assertEquals(biasTriad1, biasTriad2);
12338         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12339         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12340         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12341         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12342         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12343         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12344         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12345         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12346         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12347         final var bias1 = calibrator.getBias();
12348         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12349         final var bias2 = new double[3];
12350         calibrator.getBias(bias2);
12351         assertArrayEquals(bias1, bias2, 0.0);
12352         final var b1 = calibrator.getBiasAsMatrix();
12353         assertEquals(b1, ba);
12354         final var b2 = new Matrix(3, 1);
12355         calibrator.getBiasAsMatrix(b2);
12356         assertEquals(b1, b2);
12357         final var ma1 = calibrator.getInitialMa();
12358         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12359         final var ma2 = new Matrix(3, 3);
12360         calibrator.getInitialMa(ma2);
12361         assertEquals(ma1, ma2);
12362         assertSame(measurements, calibrator.getMeasurements());
12363         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12364                 calibrator.getMeasurementType());
12365         assertFalse(calibrator.isOrderedMeasurementsRequired());
12366         assertFalse(calibrator.isQualityScoresRequired());
12367         assertFalse(calibrator.isCommonAxisUsed());
12368         assertSame(this, calibrator.getListener());
12369         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
12370         assertFalse(calibrator.isReady());
12371         assertFalse(calibrator.isRunning());
12372         assertNull(calibrator.getEstimatedMa());
12373         assertNull(calibrator.getEstimatedSx());
12374         assertNull(calibrator.getEstimatedSy());
12375         assertNull(calibrator.getEstimatedSz());
12376         assertNull(calibrator.getEstimatedMxy());
12377         assertNull(calibrator.getEstimatedMxz());
12378         assertNull(calibrator.getEstimatedMyx());
12379         assertNull(calibrator.getEstimatedMyz());
12380         assertNull(calibrator.getEstimatedMzx());
12381         assertNull(calibrator.getEstimatedMzy());
12382         assertNull(calibrator.getEstimatedCovariance());
12383         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12384         assertNotNull(calibrator.getGroundTruthGravityNorm());
12385         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12386         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12387         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12388                 ABSOLUTE_ERROR));
12389         final var g = new Acceleration(0.0, AccelerationUnit.G);
12390         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12391         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12392         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12393 
12394         // Force IllegalArgumentException
12395         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12396                 -gravityNorm, measurements, bx, by, bz, sx, sy, sz, this));
12397     }
12398 
12399     @Test
12400     void testConstructor114() throws WrongSizeException {
12401         final var ba = generateBa();
12402         final var biasX = ba.getElementAtIndex(0);
12403         final var biasY = ba.getElementAtIndex(1);
12404         final var biasZ = ba.getElementAtIndex(2);
12405 
12406         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12407         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12408         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12409 
12410         final var ma = generateMaCommonAxis();
12411         final var sx = ma.getElementAt(0, 0);
12412         final var sy = ma.getElementAt(1, 1);
12413         final var sz = ma.getElementAt(2, 2);
12414 
12415         final var randomizer = new UniformRandomizer();
12416         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12417         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12418         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12419         final var nedPosition = new NEDPosition(latitude, longitude, height);
12420         final var nedVelocity = new NEDVelocity();
12421         final var ecefPosition = new ECEFPosition();
12422         final var ecefVelocity = new ECEFVelocity();
12423         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12424         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12425                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12426         final var gravityNorm = gravity.getNorm();
12427 
12428         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
12429                 bx, by, bz, sx, sy, sz);
12430 
12431         // check default values
12432         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12433         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12434         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12435         final var bx1 = calibrator.getBiasXAsAcceleration();
12436         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12437         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12438         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12439         calibrator.getBiasXAsAcceleration(bx2);
12440         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12441         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12442         final var by1 = calibrator.getBiasYAsAcceleration();
12443         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12444         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12445         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12446         calibrator.getBiasYAsAcceleration(by2);
12447         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12448         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12449         final var bz1 = calibrator.getBiasZAsAcceleration();
12450         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12451         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12452         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12453         calibrator.getBiasZAsAcceleration(bz2);
12454         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12455         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12456         final var biasTriad1 = calibrator.getBiasAsTriad();
12457         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12458         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12459         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12460         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12461         final var biasTriad2 = new AccelerationTriad();
12462         calibrator.getBiasAsTriad(biasTriad2);
12463         assertEquals(biasTriad1, biasTriad2);
12464         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12465         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12466         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12467         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12468         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12469         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12470         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12471         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12472         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12473         final var bias1 = calibrator.getBias();
12474         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12475         final var bias2 = new double[3];
12476         calibrator.getBias(bias2);
12477         assertArrayEquals(bias1, bias2, 0.0);
12478         final var b1 = calibrator.getBiasAsMatrix();
12479         assertEquals(b1, ba);
12480         final var b2 = new Matrix(3, 1);
12481         calibrator.getBiasAsMatrix(b2);
12482         assertEquals(b1, b2);
12483         final var ma1 = calibrator.getInitialMa();
12484         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12485         final var ma2 = new Matrix(3, 3);
12486         calibrator.getInitialMa(ma2);
12487         assertEquals(ma1, ma2);
12488         assertNull(calibrator.getMeasurements());
12489         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12490                 calibrator.getMeasurementType());
12491         assertFalse(calibrator.isOrderedMeasurementsRequired());
12492         assertFalse(calibrator.isQualityScoresRequired());
12493         assertTrue(calibrator.isCommonAxisUsed());
12494         assertNull(calibrator.getListener());
12495         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12496         assertFalse(calibrator.isReady());
12497         assertFalse(calibrator.isRunning());
12498         assertNull(calibrator.getEstimatedMa());
12499         assertNull(calibrator.getEstimatedSx());
12500         assertNull(calibrator.getEstimatedSy());
12501         assertNull(calibrator.getEstimatedSz());
12502         assertNull(calibrator.getEstimatedMxy());
12503         assertNull(calibrator.getEstimatedMxz());
12504         assertNull(calibrator.getEstimatedMyx());
12505         assertNull(calibrator.getEstimatedMyz());
12506         assertNull(calibrator.getEstimatedMzx());
12507         assertNull(calibrator.getEstimatedMzy());
12508         assertNull(calibrator.getEstimatedCovariance());
12509         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12510         assertNotNull(calibrator.getGroundTruthGravityNorm());
12511         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12512         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12513         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12514                 ABSOLUTE_ERROR));
12515         final var g = new Acceleration(0.0, AccelerationUnit.G);
12516         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12517         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12518         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12519 
12520         // Force IllegalArgumentException
12521         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12522                 -gravityNorm, true, bx, by, bz, sx, sy, sz));
12523     }
12524 
12525     @Test
12526     void testConstructor115() throws WrongSizeException {
12527         final var ba = generateBa();
12528         final var biasX = ba.getElementAtIndex(0);
12529         final var biasY = ba.getElementAtIndex(1);
12530         final var biasZ = ba.getElementAtIndex(2);
12531 
12532         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12533         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12534         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12535 
12536         final var ma = generateMaCommonAxis();
12537         final var sx = ma.getElementAt(0, 0);
12538         final var sy = ma.getElementAt(1, 1);
12539         final var sz = ma.getElementAt(2, 2);
12540 
12541         final var randomizer = new UniformRandomizer();
12542         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12543         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12544         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12545         final var nedPosition = new NEDPosition(latitude, longitude, height);
12546         final var nedVelocity = new NEDVelocity();
12547         final var ecefPosition = new ECEFPosition();
12548         final var ecefVelocity = new ECEFVelocity();
12549         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12550         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12551                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12552         final var gravityNorm = gravity.getNorm();
12553 
12554         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
12555                 bx, by, bz, sx, sy, sz, this);
12556 
12557         // check default values
12558         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12559         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12560         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12561         final var bx1 = calibrator.getBiasXAsAcceleration();
12562         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12563         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12564         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12565         calibrator.getBiasXAsAcceleration(bx2);
12566         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12567         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12568         final var by1 = calibrator.getBiasYAsAcceleration();
12569         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12570         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12571         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12572         calibrator.getBiasYAsAcceleration(by2);
12573         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12574         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12575         final var bz1 = calibrator.getBiasZAsAcceleration();
12576         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12577         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12578         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12579         calibrator.getBiasZAsAcceleration(bz2);
12580         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12581         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12582         final var biasTriad1 = calibrator.getBiasAsTriad();
12583         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12584         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12585         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12586         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12587         final var biasTriad2 = new AccelerationTriad();
12588         calibrator.getBiasAsTriad(biasTriad2);
12589         assertEquals(biasTriad1, biasTriad2);
12590         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12591         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12592         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12593         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12594         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12595         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12596         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12597         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12598         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12599         final var bias1 = calibrator.getBias();
12600         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12601         final var bias2 = new double[3];
12602         calibrator.getBias(bias2);
12603         assertArrayEquals(bias1, bias2, 0.0);
12604         final var b1 = calibrator.getBiasAsMatrix();
12605         assertEquals(b1, ba);
12606         final var b2 = new Matrix(3, 1);
12607         calibrator.getBiasAsMatrix(b2);
12608         assertEquals(b1, b2);
12609         final var ma1 = calibrator.getInitialMa();
12610         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12611         final var ma2 = new Matrix(3, 3);
12612         calibrator.getInitialMa(ma2);
12613         assertEquals(ma1, ma2);
12614         assertNull(calibrator.getMeasurements());
12615         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12616                 calibrator.getMeasurementType());
12617         assertFalse(calibrator.isOrderedMeasurementsRequired());
12618         assertFalse(calibrator.isQualityScoresRequired());
12619         assertTrue(calibrator.isCommonAxisUsed());
12620         assertSame(this, calibrator.getListener());
12621         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12622         assertFalse(calibrator.isReady());
12623         assertFalse(calibrator.isRunning());
12624         assertNull(calibrator.getEstimatedMa());
12625         assertNull(calibrator.getEstimatedSx());
12626         assertNull(calibrator.getEstimatedSy());
12627         assertNull(calibrator.getEstimatedSz());
12628         assertNull(calibrator.getEstimatedMxy());
12629         assertNull(calibrator.getEstimatedMxz());
12630         assertNull(calibrator.getEstimatedMyx());
12631         assertNull(calibrator.getEstimatedMyz());
12632         assertNull(calibrator.getEstimatedMzx());
12633         assertNull(calibrator.getEstimatedMzy());
12634         assertNull(calibrator.getEstimatedCovariance());
12635         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12636         assertNotNull(calibrator.getGroundTruthGravityNorm());
12637         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12638         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12639         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12640                 ABSOLUTE_ERROR));
12641         final var g = new Acceleration(0.0, AccelerationUnit.G);
12642         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12643         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12644         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12645 
12646         // Force IllegalArgumentException
12647         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12648                 -gravityNorm, true, bx, by, bz, sx, sy, sz, this));
12649     }
12650 
12651     @Test
12652     void testConstructor116() throws WrongSizeException {
12653         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12654 
12655         final var ba = generateBa();
12656         final var biasX = ba.getElementAtIndex(0);
12657         final var biasY = ba.getElementAtIndex(1);
12658         final var biasZ = ba.getElementAtIndex(2);
12659 
12660         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12661         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12662         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12663 
12664         final var ma = generateMaCommonAxis();
12665         final var sx = ma.getElementAt(0, 0);
12666         final var sy = ma.getElementAt(1, 1);
12667         final var sz = ma.getElementAt(2, 2);
12668 
12669         final var randomizer = new UniformRandomizer();
12670         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12671         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12672         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12673         final var nedPosition = new NEDPosition(latitude, longitude, height);
12674         final var nedVelocity = new NEDVelocity();
12675         final var ecefPosition = new ECEFPosition();
12676         final var ecefVelocity = new ECEFVelocity();
12677         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12678         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12679                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12680         final var gravityNorm = gravity.getNorm();
12681 
12682         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
12683                 true, bx, by, bz, sx, sy, sz);
12684 
12685         // check default values
12686         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12687         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12688         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12689         final var bx1 = calibrator.getBiasXAsAcceleration();
12690         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12691         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12692         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12693         calibrator.getBiasXAsAcceleration(bx2);
12694         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12695         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12696         final var by1 = calibrator.getBiasYAsAcceleration();
12697         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12698         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12699         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12700         calibrator.getBiasYAsAcceleration(by2);
12701         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12702         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12703         final var bz1 = calibrator.getBiasZAsAcceleration();
12704         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12705         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12706         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12707         calibrator.getBiasZAsAcceleration(bz2);
12708         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12709         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12710         final var biasTriad1 = calibrator.getBiasAsTriad();
12711         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12712         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12713         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12714         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12715         final var biasTriad2 = new AccelerationTriad();
12716         calibrator.getBiasAsTriad(biasTriad2);
12717         assertEquals(biasTriad1, biasTriad2);
12718         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12719         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12720         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12721         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12722         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12723         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12724         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12725         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12726         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12727         final var bias1 = calibrator.getBias();
12728         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12729         final var bias2 = new double[3];
12730         calibrator.getBias(bias2);
12731         assertArrayEquals(bias1, bias2, 0.0);
12732         final var b1 = calibrator.getBiasAsMatrix();
12733         assertEquals(b1, ba);
12734         final var b2 = new Matrix(3, 1);
12735         calibrator.getBiasAsMatrix(b2);
12736         assertEquals(b1, b2);
12737         final var ma1 = calibrator.getInitialMa();
12738         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12739         final var ma2 = new Matrix(3, 3);
12740         calibrator.getInitialMa(ma2);
12741         assertEquals(ma1, ma2);
12742         assertSame(measurements, calibrator.getMeasurements());
12743         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12744                 calibrator.getMeasurementType());
12745         assertFalse(calibrator.isOrderedMeasurementsRequired());
12746         assertFalse(calibrator.isQualityScoresRequired());
12747         assertTrue(calibrator.isCommonAxisUsed());
12748         assertNull(calibrator.getListener());
12749         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12750         assertFalse(calibrator.isReady());
12751         assertFalse(calibrator.isRunning());
12752         assertNull(calibrator.getEstimatedMa());
12753         assertNull(calibrator.getEstimatedSx());
12754         assertNull(calibrator.getEstimatedSy());
12755         assertNull(calibrator.getEstimatedSz());
12756         assertNull(calibrator.getEstimatedMxy());
12757         assertNull(calibrator.getEstimatedMxz());
12758         assertNull(calibrator.getEstimatedMyx());
12759         assertNull(calibrator.getEstimatedMyz());
12760         assertNull(calibrator.getEstimatedMzx());
12761         assertNull(calibrator.getEstimatedMzy());
12762         assertNull(calibrator.getEstimatedCovariance());
12763         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12764         assertNotNull(calibrator.getGroundTruthGravityNorm());
12765         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12766         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12767         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12768                 ABSOLUTE_ERROR));
12769         final var g = new Acceleration(0.0, AccelerationUnit.G);
12770         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12771         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12772         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12773 
12774         // Force IllegalArgumentException
12775         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12776                 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz));
12777     }
12778 
12779     @Test
12780     void testConstructor117() throws WrongSizeException {
12781         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
12782 
12783         final var ba = generateBa();
12784         final var biasX = ba.getElementAtIndex(0);
12785         final var biasY = ba.getElementAtIndex(1);
12786         final var biasZ = ba.getElementAtIndex(2);
12787 
12788         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12789         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12790         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
12791 
12792         final var ma = generateMaCommonAxis();
12793         final var sx = ma.getElementAt(0, 0);
12794         final var sy = ma.getElementAt(1, 1);
12795         final var sz = ma.getElementAt(2, 2);
12796 
12797         final var randomizer = new UniformRandomizer();
12798         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12799         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12800         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12801         final var nedPosition = new NEDPosition(latitude, longitude, height);
12802         final var nedVelocity = new NEDVelocity();
12803         final var ecefPosition = new ECEFPosition();
12804         final var ecefVelocity = new ECEFVelocity();
12805         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12806         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12807                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12808         final var gravityNorm = gravity.getNorm();
12809 
12810         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
12811                 true, bx, by, bz, sx, sy, sz, this);
12812 
12813         // check default values
12814         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12815         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12816         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12817         final var bx1 = calibrator.getBiasXAsAcceleration();
12818         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12819         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12820         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12821         calibrator.getBiasXAsAcceleration(bx2);
12822         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12823         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12824         final var by1 = calibrator.getBiasYAsAcceleration();
12825         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12826         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12827         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12828         calibrator.getBiasYAsAcceleration(by2);
12829         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12830         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12831         final var bz1 = calibrator.getBiasZAsAcceleration();
12832         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12833         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12834         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12835         calibrator.getBiasZAsAcceleration(bz2);
12836         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12837         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12838         final var biasTriad1 = calibrator.getBiasAsTriad();
12839         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12840         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12841         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12842         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12843         final var biasTriad2 = new AccelerationTriad();
12844         calibrator.getBiasAsTriad(biasTriad2);
12845         assertEquals(biasTriad1, biasTriad2);
12846         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12847         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12848         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12849         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
12850         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
12851         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
12852         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
12853         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
12854         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
12855         final var bias1 = calibrator.getBias();
12856         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12857         final var bias2 = new double[3];
12858         calibrator.getBias(bias2);
12859         assertArrayEquals(bias1, bias2, 0.0);
12860         final var b1 = calibrator.getBiasAsMatrix();
12861         assertEquals(b1, ba);
12862         final var b2 = new Matrix(3, 1);
12863         calibrator.getBiasAsMatrix(b2);
12864         assertEquals(b1, b2);
12865         final var ma1 = calibrator.getInitialMa();
12866         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12867         final var ma2 = new Matrix(3, 3);
12868         calibrator.getInitialMa(ma2);
12869         assertEquals(ma1, ma2);
12870         assertSame(measurements, calibrator.getMeasurements());
12871         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
12872                 calibrator.getMeasurementType());
12873         assertFalse(calibrator.isOrderedMeasurementsRequired());
12874         assertFalse(calibrator.isQualityScoresRequired());
12875         assertTrue(calibrator.isCommonAxisUsed());
12876         assertSame(this, calibrator.getListener());
12877         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
12878         assertFalse(calibrator.isReady());
12879         assertFalse(calibrator.isRunning());
12880         assertNull(calibrator.getEstimatedMa());
12881         assertNull(calibrator.getEstimatedSx());
12882         assertNull(calibrator.getEstimatedSy());
12883         assertNull(calibrator.getEstimatedSz());
12884         assertNull(calibrator.getEstimatedMxy());
12885         assertNull(calibrator.getEstimatedMxz());
12886         assertNull(calibrator.getEstimatedMyx());
12887         assertNull(calibrator.getEstimatedMyz());
12888         assertNull(calibrator.getEstimatedMzx());
12889         assertNull(calibrator.getEstimatedMzy());
12890         assertNull(calibrator.getEstimatedCovariance());
12891         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
12892         assertNotNull(calibrator.getGroundTruthGravityNorm());
12893         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12894         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12895         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
12896                 ABSOLUTE_ERROR));
12897         final var g = new Acceleration(0.0, AccelerationUnit.G);
12898         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12899         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12900         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
12901 
12902         // Force IllegalArgumentException
12903         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
12904                 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz, this));
12905     }
12906 
12907     @Test
12908     void testConstructor118() throws WrongSizeException {
12909         final var ba = generateBa();
12910         final var biasX = ba.getElementAtIndex(0);
12911         final var biasY = ba.getElementAtIndex(1);
12912         final var biasZ = ba.getElementAtIndex(2);
12913 
12914         final var ma = generateMaCommonAxis();
12915         final var sx = ma.getElementAt(0, 0);
12916         final var sy = ma.getElementAt(1, 1);
12917         final var sz = ma.getElementAt(2, 2);
12918         final var mxy = ma.getElementAt(0, 1);
12919         final var mxz = ma.getElementAt(0, 2);
12920         final var myx = ma.getElementAt(1, 0);
12921         final var myz = ma.getElementAt(1, 2);
12922         final var mzx = ma.getElementAt(2, 0);
12923         final var mzy = ma.getElementAt(2, 1);
12924 
12925         final var randomizer = new UniformRandomizer();
12926         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
12927         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12928         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12929         final var nedPosition = new NEDPosition(latitude, longitude, height);
12930         final var nedVelocity = new NEDVelocity();
12931         final var ecefPosition = new ECEFPosition();
12932         final var ecefVelocity = new ECEFVelocity();
12933         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
12934         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12935                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12936         final var gravityNorm = gravity.getNorm();
12937 
12938         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
12939                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
12940 
12941         // check default values
12942         assertEquals(biasX, calibrator.getBiasX(), 0.0);
12943         assertEquals(biasY, calibrator.getBiasY(), 0.0);
12944         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
12945         final var bx1 = calibrator.getBiasXAsAcceleration();
12946         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
12947         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
12948         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12949         calibrator.getBiasXAsAcceleration(bx2);
12950         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
12951         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
12952         final var by1 = calibrator.getBiasYAsAcceleration();
12953         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
12954         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
12955         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12956         calibrator.getBiasYAsAcceleration(by2);
12957         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
12958         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
12959         final var bz1 = calibrator.getBiasZAsAcceleration();
12960         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
12961         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
12962         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
12963         calibrator.getBiasZAsAcceleration(bz2);
12964         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
12965         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
12966         final var biasTriad1 = calibrator.getBiasAsTriad();
12967         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
12968         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
12969         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
12970         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
12971         final var biasTriad2 = new AccelerationTriad();
12972         calibrator.getBiasAsTriad(biasTriad2);
12973         assertEquals(biasTriad1, biasTriad2);
12974         assertEquals(sx, calibrator.getInitialSx(), 0.0);
12975         assertEquals(sy, calibrator.getInitialSy(), 0.0);
12976         assertEquals(sz, calibrator.getInitialSz(), 0.0);
12977         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
12978         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
12979         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
12980         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
12981         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
12982         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
12983         final var bias1 = calibrator.getBias();
12984         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12985         final var bias2 = new double[3];
12986         calibrator.getBias(bias2);
12987         assertArrayEquals(bias1, bias2, 0.0);
12988         final var b1 = calibrator.getBiasAsMatrix();
12989         assertEquals(b1, ba);
12990         final var b2 = new Matrix(3, 1);
12991         calibrator.getBiasAsMatrix(b2);
12992         assertEquals(b1, b2);
12993         final var ma1 = new Matrix(3, 3);
12994         ma1.setSubmatrix(0, 0, 2, 2,
12995                 new double[]{sx, myx, mzx,
12996                         mxy, sy, mzy,
12997                         mxz, myz, sz});
12998         assertEquals(ma1, calibrator.getInitialMa());
12999         final var ma2 = new Matrix(3, 3);
13000         calibrator.getInitialMa(ma2);
13001         assertEquals(ma1, ma2);
13002         assertNull(calibrator.getMeasurements());
13003         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13004                 calibrator.getMeasurementType());
13005         assertFalse(calibrator.isOrderedMeasurementsRequired());
13006         assertFalse(calibrator.isQualityScoresRequired());
13007         assertFalse(calibrator.isCommonAxisUsed());
13008         assertNull(calibrator.getListener());
13009         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13010         assertFalse(calibrator.isReady());
13011         assertFalse(calibrator.isRunning());
13012         assertNull(calibrator.getEstimatedMa());
13013         assertNull(calibrator.getEstimatedSx());
13014         assertNull(calibrator.getEstimatedSy());
13015         assertNull(calibrator.getEstimatedSz());
13016         assertNull(calibrator.getEstimatedMxy());
13017         assertNull(calibrator.getEstimatedMxz());
13018         assertNull(calibrator.getEstimatedMyx());
13019         assertNull(calibrator.getEstimatedMyz());
13020         assertNull(calibrator.getEstimatedMzx());
13021         assertNull(calibrator.getEstimatedMzy());
13022         assertNull(calibrator.getEstimatedCovariance());
13023         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13024         assertNotNull(calibrator.getGroundTruthGravityNorm());
13025         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13026         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13027         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13028                 ABSOLUTE_ERROR));
13029         final var g = new Acceleration(0.0, AccelerationUnit.G);
13030         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13031         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13032         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13033 
13034         // Force IllegalArgumentException
13035         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13036                 -gravityNorm, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13037     }
13038 
13039     @Test
13040     void testConstructor119() throws WrongSizeException {
13041         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13042 
13043         final var ba = generateBa();
13044         final var biasX = ba.getElementAtIndex(0);
13045         final var biasY = ba.getElementAtIndex(1);
13046         final var biasZ = ba.getElementAtIndex(2);
13047 
13048         final var ma = generateMaCommonAxis();
13049         final var sx = ma.getElementAt(0, 0);
13050         final var sy = ma.getElementAt(1, 1);
13051         final var sz = ma.getElementAt(2, 2);
13052         final var mxy = ma.getElementAt(0, 1);
13053         final var mxz = ma.getElementAt(0, 2);
13054         final var myx = ma.getElementAt(1, 0);
13055         final var myz = ma.getElementAt(1, 2);
13056         final var mzx = ma.getElementAt(2, 0);
13057         final var mzy = ma.getElementAt(2, 1);
13058 
13059         final var randomizer = new UniformRandomizer();
13060         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13061         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13062         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13063         final var nedPosition = new NEDPosition(latitude, longitude, height);
13064         final var nedVelocity = new NEDVelocity();
13065         final var ecefPosition = new ECEFPosition();
13066         final var ecefVelocity = new ECEFVelocity();
13067         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13068         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13069                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13070         final var gravityNorm = gravity.getNorm();
13071 
13072         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13073                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13074 
13075         // check default values
13076         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13077         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13078         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13079         final var bx1 = calibrator.getBiasXAsAcceleration();
13080         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13081         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13082         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13083         calibrator.getBiasXAsAcceleration(bx2);
13084         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13085         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13086         final var by1 = calibrator.getBiasYAsAcceleration();
13087         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13088         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13089         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13090         calibrator.getBiasYAsAcceleration(by2);
13091         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13092         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13093         final var bz1 = calibrator.getBiasZAsAcceleration();
13094         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13095         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13096         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13097         calibrator.getBiasZAsAcceleration(bz2);
13098         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13099         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13100         final var biasTriad1 = calibrator.getBiasAsTriad();
13101         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13102         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13103         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13104         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13105         final var biasTriad2 = new AccelerationTriad();
13106         calibrator.getBiasAsTriad(biasTriad2);
13107         assertEquals(biasTriad1, biasTriad2);
13108         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13109         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13110         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13111         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13112         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13113         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13114         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13115         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13116         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13117         final var bias1 = calibrator.getBias();
13118         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13119         final var bias2 = new double[3];
13120         calibrator.getBias(bias2);
13121         assertArrayEquals(bias1, bias2, 0.0);
13122         final var b1 = calibrator.getBiasAsMatrix();
13123         assertEquals(b1, ba);
13124         final var b2 = new Matrix(3, 1);
13125         calibrator.getBiasAsMatrix(b2);
13126         assertEquals(b1, b2);
13127         final var ma1 = new Matrix(3, 3);
13128         ma1.setSubmatrix(0, 0, 2, 2,
13129                 new double[]{sx, myx, mzx,
13130                         mxy, sy, mzy,
13131                         mxz, myz, sz});
13132         assertEquals(ma1, calibrator.getInitialMa());
13133         final var ma2 = new Matrix(3, 3);
13134         calibrator.getInitialMa(ma2);
13135         assertEquals(ma1, ma2);
13136         assertSame(measurements, calibrator.getMeasurements());
13137         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13138                 calibrator.getMeasurementType());
13139         assertFalse(calibrator.isOrderedMeasurementsRequired());
13140         assertFalse(calibrator.isQualityScoresRequired());
13141         assertFalse(calibrator.isCommonAxisUsed());
13142         assertNull(calibrator.getListener());
13143         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13144         assertFalse(calibrator.isReady());
13145         assertFalse(calibrator.isRunning());
13146         assertNull(calibrator.getEstimatedMa());
13147         assertNull(calibrator.getEstimatedSx());
13148         assertNull(calibrator.getEstimatedSy());
13149         assertNull(calibrator.getEstimatedSz());
13150         assertNull(calibrator.getEstimatedMxy());
13151         assertNull(calibrator.getEstimatedMxz());
13152         assertNull(calibrator.getEstimatedMyx());
13153         assertNull(calibrator.getEstimatedMyz());
13154         assertNull(calibrator.getEstimatedMzx());
13155         assertNull(calibrator.getEstimatedMzy());
13156         assertNull(calibrator.getEstimatedCovariance());
13157         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13158         assertNotNull(calibrator.getGroundTruthGravityNorm());
13159         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13160         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13161         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13162                 ABSOLUTE_ERROR));
13163         final var g = new Acceleration(0.0, AccelerationUnit.G);
13164         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13165         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13166         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13167 
13168         // Force IllegalArgumentException
13169         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13170                 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13171     }
13172 
13173     @Test
13174     void testConstructor120() throws WrongSizeException {
13175         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13176 
13177         final var ba = generateBa();
13178         final var biasX = ba.getElementAtIndex(0);
13179         final var biasY = ba.getElementAtIndex(1);
13180         final var biasZ = ba.getElementAtIndex(2);
13181 
13182         final var ma = generateMaCommonAxis();
13183         final var sx = ma.getElementAt(0, 0);
13184         final var sy = ma.getElementAt(1, 1);
13185         final var sz = ma.getElementAt(2, 2);
13186         final var mxy = ma.getElementAt(0, 1);
13187         final var mxz = ma.getElementAt(0, 2);
13188         final var myx = ma.getElementAt(1, 0);
13189         final var myz = ma.getElementAt(1, 2);
13190         final var mzx = ma.getElementAt(2, 0);
13191         final var mzy = ma.getElementAt(2, 1);
13192 
13193         final var randomizer = new UniformRandomizer();
13194         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13195         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13196         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13197         final var nedPosition = new NEDPosition(latitude, longitude, height);
13198         final var nedVelocity = new NEDVelocity();
13199         final var ecefPosition = new ECEFPosition();
13200         final var ecefVelocity = new ECEFVelocity();
13201         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13202         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13203                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13204         final var gravityNorm = gravity.getNorm();
13205 
13206         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13207                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
13208 
13209         // check default values
13210         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13211         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13212         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13213         final var bx1 = calibrator.getBiasXAsAcceleration();
13214         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13215         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13216         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13217         calibrator.getBiasXAsAcceleration(bx2);
13218         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13219         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13220         final var by1 = calibrator.getBiasYAsAcceleration();
13221         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13222         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13223         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13224         calibrator.getBiasYAsAcceleration(by2);
13225         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13226         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13227         final var bz1 = calibrator.getBiasZAsAcceleration();
13228         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13229         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13230         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13231         calibrator.getBiasZAsAcceleration(bz2);
13232         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13233         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13234         final var biasTriad1 = calibrator.getBiasAsTriad();
13235         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13236         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13237         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13238         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13239         final var biasTriad2 = new AccelerationTriad();
13240         calibrator.getBiasAsTriad(biasTriad2);
13241         assertEquals(biasTriad1, biasTriad2);
13242         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13243         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13244         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13245         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13246         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13247         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13248         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13249         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13250         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13251         final var bias1 = calibrator.getBias();
13252         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13253         final var bias2 = new double[3];
13254         calibrator.getBias(bias2);
13255         assertArrayEquals(bias1, bias2, 0.0);
13256         final var b1 = calibrator.getBiasAsMatrix();
13257         assertEquals(b1, ba);
13258         final var b2 = new Matrix(3, 1);
13259         calibrator.getBiasAsMatrix(b2);
13260         assertEquals(b1, b2);
13261         final var ma1 = new Matrix(3, 3);
13262         ma1.setSubmatrix(0, 0, 2, 2,
13263                 new double[]{sx, myx, mzx,
13264                         mxy, sy, mzy,
13265                         mxz, myz, sz});
13266         assertEquals(ma1, calibrator.getInitialMa());
13267         final var ma2 = new Matrix(3, 3);
13268         calibrator.getInitialMa(ma2);
13269         assertEquals(ma1, ma2);
13270         assertSame(measurements, calibrator.getMeasurements());
13271         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13272                 calibrator.getMeasurementType());
13273         assertFalse(calibrator.isOrderedMeasurementsRequired());
13274         assertFalse(calibrator.isQualityScoresRequired());
13275         assertFalse(calibrator.isCommonAxisUsed());
13276         assertSame(this, calibrator.getListener());
13277         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13278         assertFalse(calibrator.isReady());
13279         assertFalse(calibrator.isRunning());
13280         assertNull(calibrator.getEstimatedMa());
13281         assertNull(calibrator.getEstimatedSx());
13282         assertNull(calibrator.getEstimatedSy());
13283         assertNull(calibrator.getEstimatedSz());
13284         assertNull(calibrator.getEstimatedMxy());
13285         assertNull(calibrator.getEstimatedMxz());
13286         assertNull(calibrator.getEstimatedMyx());
13287         assertNull(calibrator.getEstimatedMyz());
13288         assertNull(calibrator.getEstimatedMzx());
13289         assertNull(calibrator.getEstimatedMzy());
13290         assertNull(calibrator.getEstimatedCovariance());
13291         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13292         assertNotNull(calibrator.getGroundTruthGravityNorm());
13293         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13294         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13295         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13296                 ABSOLUTE_ERROR));
13297         final var g = new Acceleration(0.0, AccelerationUnit.G);
13298         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13299         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13300         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13301 
13302         // Force IllegalArgumentException
13303         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13304                 -gravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
13305                 this));
13306     }
13307 
13308     @Test
13309     void testConstructor121() throws WrongSizeException {
13310         final var ba = generateBa();
13311         final var biasX = ba.getElementAtIndex(0);
13312         final var biasY = ba.getElementAtIndex(1);
13313         final var biasZ = ba.getElementAtIndex(2);
13314 
13315         final var ma = generateMaCommonAxis();
13316         final var sx = ma.getElementAt(0, 0);
13317         final var sy = ma.getElementAt(1, 1);
13318         final var sz = ma.getElementAt(2, 2);
13319         final var mxy = ma.getElementAt(0, 1);
13320         final var mxz = ma.getElementAt(0, 2);
13321         final var myx = ma.getElementAt(1, 0);
13322         final var myz = ma.getElementAt(1, 2);
13323         final var mzx = ma.getElementAt(2, 0);
13324         final var mzy = ma.getElementAt(2, 1);
13325 
13326         final var randomizer = new UniformRandomizer();
13327         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13328         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13329         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13330         final var nedPosition = new NEDPosition(latitude, longitude, height);
13331         final var nedVelocity = new NEDVelocity();
13332         final var ecefPosition = new ECEFPosition();
13333         final var ecefVelocity = new ECEFVelocity();
13334         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13335         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13336                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13337         final var gravityNorm = gravity.getNorm();
13338 
13339         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
13340                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13341 
13342         // check default values
13343         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13344         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13345         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13346         final var bx1 = calibrator.getBiasXAsAcceleration();
13347         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13348         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13349         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13350         calibrator.getBiasXAsAcceleration(bx2);
13351         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13352         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13353         final var by1 = calibrator.getBiasYAsAcceleration();
13354         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13355         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13356         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13357         calibrator.getBiasYAsAcceleration(by2);
13358         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13359         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13360         final var bz1 = calibrator.getBiasZAsAcceleration();
13361         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13362         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13363         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13364         calibrator.getBiasZAsAcceleration(bz2);
13365         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13366         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13367         final var biasTriad1 = calibrator.getBiasAsTriad();
13368         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13369         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13370         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13371         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13372         final var biasTriad2 = new AccelerationTriad();
13373         calibrator.getBiasAsTriad(biasTriad2);
13374         assertEquals(biasTriad1, biasTriad2);
13375         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13376         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13377         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13378         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13379         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13380         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13381         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13382         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13383         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13384         final var bias1 = calibrator.getBias();
13385         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13386         final var bias2 = new double[3];
13387         calibrator.getBias(bias2);
13388         assertArrayEquals(bias1, bias2, 0.0);
13389         final var b1 = calibrator.getBiasAsMatrix();
13390         assertEquals(b1, ba);
13391         final var b2 = new Matrix(3, 1);
13392         calibrator.getBiasAsMatrix(b2);
13393         assertEquals(b1, b2);
13394         final var ma1 = new Matrix(3, 3);
13395         ma1.setSubmatrix(0, 0, 2, 2,
13396                 new double[]{sx, myx, mzx,
13397                         mxy, sy, mzy,
13398                         mxz, myz, sz});
13399         assertEquals(ma1, calibrator.getInitialMa());
13400         final var ma2 = new Matrix(3, 3);
13401         calibrator.getInitialMa(ma2);
13402         assertEquals(ma1, ma2);
13403         assertNull(calibrator.getMeasurements());
13404         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13405                 calibrator.getMeasurementType());
13406         assertFalse(calibrator.isOrderedMeasurementsRequired());
13407         assertFalse(calibrator.isQualityScoresRequired());
13408         assertTrue(calibrator.isCommonAxisUsed());
13409         assertNull(calibrator.getListener());
13410         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13411         assertFalse(calibrator.isReady());
13412         assertFalse(calibrator.isRunning());
13413         assertNull(calibrator.getEstimatedMa());
13414         assertNull(calibrator.getEstimatedSx());
13415         assertNull(calibrator.getEstimatedSy());
13416         assertNull(calibrator.getEstimatedSz());
13417         assertNull(calibrator.getEstimatedMxy());
13418         assertNull(calibrator.getEstimatedMxz());
13419         assertNull(calibrator.getEstimatedMyx());
13420         assertNull(calibrator.getEstimatedMyz());
13421         assertNull(calibrator.getEstimatedMzx());
13422         assertNull(calibrator.getEstimatedMzy());
13423         assertNull(calibrator.getEstimatedCovariance());
13424         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13425         assertNotNull(calibrator.getGroundTruthGravityNorm());
13426         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13427         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13428         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13429                 ABSOLUTE_ERROR));
13430         final var g = new Acceleration(0.0, AccelerationUnit.G);
13431         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13432         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13433         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13434 
13435         // Force IllegalArgumentException
13436         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13437                 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13438     }
13439 
13440     @Test
13441     void testConstructor122() throws WrongSizeException {
13442         final var ba = generateBa();
13443         final var biasX = ba.getElementAtIndex(0);
13444         final var biasY = ba.getElementAtIndex(1);
13445         final var biasZ = ba.getElementAtIndex(2);
13446 
13447         final var ma = generateMaCommonAxis();
13448         final var sx = ma.getElementAt(0, 0);
13449         final var sy = ma.getElementAt(1, 1);
13450         final var sz = ma.getElementAt(2, 2);
13451         final var mxy = ma.getElementAt(0, 1);
13452         final var mxz = ma.getElementAt(0, 2);
13453         final var myx = ma.getElementAt(1, 0);
13454         final var myz = ma.getElementAt(1, 2);
13455         final var mzx = ma.getElementAt(2, 0);
13456         final var mzy = ma.getElementAt(2, 1);
13457 
13458         final var randomizer = new UniformRandomizer();
13459         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13460         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13461         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13462         final var nedPosition = new NEDPosition(latitude, longitude, height);
13463         final var nedVelocity = new NEDVelocity();
13464         final var ecefPosition = new ECEFPosition();
13465         final var ecefVelocity = new ECEFVelocity();
13466         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13467         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13468                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13469         final var gravityNorm = gravity.getNorm();
13470 
13471         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
13472                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
13473 
13474         // check default values
13475         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13476         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13477         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13478         final var bx1 = calibrator.getBiasXAsAcceleration();
13479         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13480         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13481         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13482         calibrator.getBiasXAsAcceleration(bx2);
13483         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13484         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13485         final var by1 = calibrator.getBiasYAsAcceleration();
13486         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13487         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13488         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13489         calibrator.getBiasYAsAcceleration(by2);
13490         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13491         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13492         final var bz1 = calibrator.getBiasZAsAcceleration();
13493         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13494         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13495         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13496         calibrator.getBiasZAsAcceleration(bz2);
13497         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13498         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13499         final var biasTriad1 = calibrator.getBiasAsTriad();
13500         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13501         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13502         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13503         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13504         final var biasTriad2 = new AccelerationTriad();
13505         calibrator.getBiasAsTriad(biasTriad2);
13506         assertEquals(biasTriad1, biasTriad2);
13507         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13508         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13509         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13510         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13511         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13512         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13513         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13514         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13515         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13516         final var bias1 = calibrator.getBias();
13517         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13518         final var bias2 = new double[3];
13519         calibrator.getBias(bias2);
13520         assertArrayEquals(bias1, bias2, 0.0);
13521         final var b1 = calibrator.getBiasAsMatrix();
13522         assertEquals(b1, ba);
13523         final var b2 = new Matrix(3, 1);
13524         calibrator.getBiasAsMatrix(b2);
13525         assertEquals(b1, b2);
13526         final var ma1 = new Matrix(3, 3);
13527         ma1.setSubmatrix(0, 0, 2, 2,
13528                 new double[]{sx, myx, mzx,
13529                         mxy, sy, mzy,
13530                         mxz, myz, sz});
13531         assertEquals(ma1, calibrator.getInitialMa());
13532         final var ma2 = new Matrix(3, 3);
13533         calibrator.getInitialMa(ma2);
13534         assertEquals(ma1, ma2);
13535         assertNull(calibrator.getMeasurements());
13536         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13537                 calibrator.getMeasurementType());
13538         assertFalse(calibrator.isOrderedMeasurementsRequired());
13539         assertFalse(calibrator.isQualityScoresRequired());
13540         assertTrue(calibrator.isCommonAxisUsed());
13541         assertSame(this, calibrator.getListener());
13542         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13543         assertFalse(calibrator.isReady());
13544         assertFalse(calibrator.isRunning());
13545         assertNull(calibrator.getEstimatedMa());
13546         assertNull(calibrator.getEstimatedSx());
13547         assertNull(calibrator.getEstimatedSy());
13548         assertNull(calibrator.getEstimatedSz());
13549         assertNull(calibrator.getEstimatedMxy());
13550         assertNull(calibrator.getEstimatedMxz());
13551         assertNull(calibrator.getEstimatedMyx());
13552         assertNull(calibrator.getEstimatedMyz());
13553         assertNull(calibrator.getEstimatedMzx());
13554         assertNull(calibrator.getEstimatedMzy());
13555         assertNull(calibrator.getEstimatedCovariance());
13556         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13557         assertNotNull(calibrator.getGroundTruthGravityNorm());
13558         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13559         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13560         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13561                 ABSOLUTE_ERROR));
13562         final var g = new Acceleration(0.0, AccelerationUnit.G);
13563         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13564         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13565         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13566 
13567         // Force IllegalArgumentException
13568         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13569                 -gravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
13570     }
13571 
13572     @Test
13573     void testConstructor123() throws WrongSizeException {
13574         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13575 
13576         final var ba = generateBa();
13577         final var biasX = ba.getElementAtIndex(0);
13578         final var biasY = ba.getElementAtIndex(1);
13579         final var biasZ = ba.getElementAtIndex(2);
13580 
13581         final var ma = generateMaCommonAxis();
13582         final var sx = ma.getElementAt(0, 0);
13583         final var sy = ma.getElementAt(1, 1);
13584         final var sz = ma.getElementAt(2, 2);
13585         final var mxy = ma.getElementAt(0, 1);
13586         final var mxz = ma.getElementAt(0, 2);
13587         final var myx = ma.getElementAt(1, 0);
13588         final var myz = ma.getElementAt(1, 2);
13589         final var mzx = ma.getElementAt(2, 0);
13590         final var mzy = ma.getElementAt(2, 1);
13591 
13592         final var randomizer = new UniformRandomizer();
13593         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13594         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13595         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13596         final var nedPosition = new NEDPosition(latitude, longitude, height);
13597         final var nedVelocity = new NEDVelocity();
13598         final var ecefPosition = new ECEFPosition();
13599         final var ecefVelocity = new ECEFVelocity();
13600         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13601         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13602                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13603         final var gravityNorm = gravity.getNorm();
13604 
13605         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13606                 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
13607 
13608         // check default values
13609         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13610         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13611         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13612         final var bx1 = calibrator.getBiasXAsAcceleration();
13613         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13614         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13615         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13616         calibrator.getBiasXAsAcceleration(bx2);
13617         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13618         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13619         final var by1 = calibrator.getBiasYAsAcceleration();
13620         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13621         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13622         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13623         calibrator.getBiasYAsAcceleration(by2);
13624         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13625         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13626         final var bz1 = calibrator.getBiasZAsAcceleration();
13627         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13628         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13629         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13630         calibrator.getBiasZAsAcceleration(bz2);
13631         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13632         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13633         final var biasTriad1 = calibrator.getBiasAsTriad();
13634         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13635         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13636         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13637         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13638         final var biasTriad2 = new AccelerationTriad();
13639         calibrator.getBiasAsTriad(biasTriad2);
13640         assertEquals(biasTriad1, biasTriad2);
13641         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13642         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13643         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13644         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13645         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13646         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13647         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13648         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13649         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13650         final var bias1 = calibrator.getBias();
13651         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13652         final var bias2 = new double[3];
13653         calibrator.getBias(bias2);
13654         assertArrayEquals(bias1, bias2, 0.0);
13655         final var b1 = calibrator.getBiasAsMatrix();
13656         assertEquals(b1, ba);
13657         final var b2 = new Matrix(3, 1);
13658         calibrator.getBiasAsMatrix(b2);
13659         assertEquals(b1, b2);
13660         final var ma1 = new Matrix(3, 3);
13661         ma1.setSubmatrix(0, 0, 2, 2,
13662                 new double[]{sx, myx, mzx,
13663                         mxy, sy, mzy,
13664                         mxz, myz, sz});
13665         assertEquals(ma1, calibrator.getInitialMa());
13666         final var ma2 = new Matrix(3, 3);
13667         calibrator.getInitialMa(ma2);
13668         assertEquals(ma1, ma2);
13669         assertSame(measurements, calibrator.getMeasurements());
13670         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13671                 calibrator.getMeasurementType());
13672         assertFalse(calibrator.isOrderedMeasurementsRequired());
13673         assertFalse(calibrator.isQualityScoresRequired());
13674         assertTrue(calibrator.isCommonAxisUsed());
13675         assertNull(calibrator.getListener());
13676         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13677         assertFalse(calibrator.isReady());
13678         assertFalse(calibrator.isRunning());
13679         assertNull(calibrator.getEstimatedMa());
13680         assertNull(calibrator.getEstimatedSx());
13681         assertNull(calibrator.getEstimatedSy());
13682         assertNull(calibrator.getEstimatedSz());
13683         assertNull(calibrator.getEstimatedMxy());
13684         assertNull(calibrator.getEstimatedMxz());
13685         assertNull(calibrator.getEstimatedMyx());
13686         assertNull(calibrator.getEstimatedMyz());
13687         assertNull(calibrator.getEstimatedMzx());
13688         assertNull(calibrator.getEstimatedMzy());
13689         assertNull(calibrator.getEstimatedCovariance());
13690         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13691         assertNotNull(calibrator.getGroundTruthGravityNorm());
13692         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13693         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13694         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13695                 ABSOLUTE_ERROR));
13696         final var g = new Acceleration(0.0, AccelerationUnit.G);
13697         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13698         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13699         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13700 
13701         // Force IllegalArgumentException
13702         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13703                 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
13704                 myz, mzx, mzy));
13705     }
13706 
13707     @Test
13708     void testConstructor124() throws WrongSizeException {
13709         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
13710 
13711         final var ba = generateBa();
13712         final var biasX = ba.getElementAtIndex(0);
13713         final var biasY = ba.getElementAtIndex(1);
13714         final var biasZ = ba.getElementAtIndex(2);
13715 
13716         final var ma = generateMaCommonAxis();
13717         final var sx = ma.getElementAt(0, 0);
13718         final var sy = ma.getElementAt(1, 1);
13719         final var sz = ma.getElementAt(2, 2);
13720         final var mxy = ma.getElementAt(0, 1);
13721         final var mxz = ma.getElementAt(0, 2);
13722         final var myx = ma.getElementAt(1, 0);
13723         final var myz = ma.getElementAt(1, 2);
13724         final var mzx = ma.getElementAt(2, 0);
13725         final var mzy = ma.getElementAt(2, 1);
13726 
13727         final var randomizer = new UniformRandomizer();
13728         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13729         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13730         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13731         final var nedPosition = new NEDPosition(latitude, longitude, height);
13732         final var nedVelocity = new NEDVelocity();
13733         final var ecefPosition = new ECEFPosition();
13734         final var ecefVelocity = new ECEFVelocity();
13735         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13736         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13737                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13738         final var gravityNorm = gravity.getNorm();
13739 
13740         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
13741                 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
13742 
13743         // check default values
13744         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13745         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13746         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13747         final var bx1 = calibrator.getBiasXAsAcceleration();
13748         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13749         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13750         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13751         calibrator.getBiasXAsAcceleration(bx2);
13752         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13753         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13754         final var by1 = calibrator.getBiasYAsAcceleration();
13755         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13756         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13757         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13758         calibrator.getBiasYAsAcceleration(by2);
13759         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13760         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13761         final var bz1 = calibrator.getBiasZAsAcceleration();
13762         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13763         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13764         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13765         calibrator.getBiasZAsAcceleration(bz2);
13766         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13767         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13768         final var biasTriad1 = calibrator.getBiasAsTriad();
13769         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13770         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13771         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13772         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13773         final var biasTriad2 = new AccelerationTriad();
13774         calibrator.getBiasAsTriad(biasTriad2);
13775         assertEquals(biasTriad1, biasTriad2);
13776         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13777         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13778         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13779         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13780         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13781         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13782         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13783         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13784         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13785         final var bias1 = calibrator.getBias();
13786         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13787         final var bias2 = new double[3];
13788         calibrator.getBias(bias2);
13789         assertArrayEquals(bias1, bias2, 0.0);
13790         final var b1 = calibrator.getBiasAsMatrix();
13791         assertEquals(b1, ba);
13792         final var b2 = new Matrix(3, 1);
13793         calibrator.getBiasAsMatrix(b2);
13794         assertEquals(b1, b2);
13795         final var ma1 = new Matrix(3, 3);
13796         ma1.setSubmatrix(0, 0, 2, 2,
13797                 new double[]{sx, myx, mzx,
13798                         mxy, sy, mzy,
13799                         mxz, myz, sz});
13800         assertEquals(ma1, calibrator.getInitialMa());
13801         final var ma2 = new Matrix(3, 3);
13802         calibrator.getInitialMa(ma2);
13803         assertEquals(ma1, ma2);
13804         assertSame(measurements, calibrator.getMeasurements());
13805         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13806                 calibrator.getMeasurementType());
13807         assertFalse(calibrator.isOrderedMeasurementsRequired());
13808         assertFalse(calibrator.isQualityScoresRequired());
13809         assertTrue(calibrator.isCommonAxisUsed());
13810         assertSame(this, calibrator.getListener());
13811         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
13812         assertFalse(calibrator.isReady());
13813         assertFalse(calibrator.isRunning());
13814         assertNull(calibrator.getEstimatedMa());
13815         assertNull(calibrator.getEstimatedSx());
13816         assertNull(calibrator.getEstimatedSy());
13817         assertNull(calibrator.getEstimatedSz());
13818         assertNull(calibrator.getEstimatedMxy());
13819         assertNull(calibrator.getEstimatedMxz());
13820         assertNull(calibrator.getEstimatedMyx());
13821         assertNull(calibrator.getEstimatedMyz());
13822         assertNull(calibrator.getEstimatedMzx());
13823         assertNull(calibrator.getEstimatedMzy());
13824         assertNull(calibrator.getEstimatedCovariance());
13825         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13826         assertNotNull(calibrator.getGroundTruthGravityNorm());
13827         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13828         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13829         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13830                 ABSOLUTE_ERROR));
13831         final var g = new Acceleration(0.0, AccelerationUnit.G);
13832         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13833         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13834         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13835 
13836         // Force IllegalArgumentException
13837         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13838                 -gravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
13839     }
13840 
13841     @Test
13842     void testConstructor125() throws WrongSizeException {
13843         final var ba = generateBa();
13844         final var biasX = ba.getElementAtIndex(0);
13845         final var biasY = ba.getElementAtIndex(1);
13846         final var biasZ = ba.getElementAtIndex(2);
13847 
13848         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13849         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13850         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13851 
13852         final var ma = generateMaCommonAxis();
13853         final var sx = ma.getElementAt(0, 0);
13854         final var sy = ma.getElementAt(1, 1);
13855         final var sz = ma.getElementAt(2, 2);
13856         final var mxy = ma.getElementAt(0, 1);
13857         final var mxz = ma.getElementAt(0, 2);
13858         final var myx = ma.getElementAt(1, 0);
13859         final var myz = ma.getElementAt(1, 2);
13860         final var mzx = ma.getElementAt(2, 0);
13861         final var mzy = ma.getElementAt(2, 1);
13862 
13863         final var randomizer = new UniformRandomizer();
13864         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
13865         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13866         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13867         final var nedPosition = new NEDPosition(latitude, longitude, height);
13868         final var nedVelocity = new NEDVelocity();
13869         final var ecefPosition = new ECEFPosition();
13870         final var ecefVelocity = new ECEFVelocity();
13871         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
13872         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13873                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13874         final var gravityNorm = gravity.getNorm();
13875 
13876         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
13877                 mxy, mxz, myx, myz, mzx, mzy);
13878 
13879         // check default values
13880         assertEquals(biasX, calibrator.getBiasX(), 0.0);
13881         assertEquals(biasY, calibrator.getBiasY(), 0.0);
13882         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
13883         final var bx1 = calibrator.getBiasXAsAcceleration();
13884         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
13885         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
13886         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13887         calibrator.getBiasXAsAcceleration(bx2);
13888         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
13889         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
13890         final var by1 = calibrator.getBiasYAsAcceleration();
13891         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
13892         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
13893         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13894         calibrator.getBiasYAsAcceleration(by2);
13895         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
13896         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
13897         final var bz1 = calibrator.getBiasZAsAcceleration();
13898         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
13899         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
13900         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
13901         calibrator.getBiasZAsAcceleration(bz2);
13902         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
13903         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
13904         final var biasTriad1 = calibrator.getBiasAsTriad();
13905         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
13906         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
13907         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
13908         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
13909         final var biasTriad2 = new AccelerationTriad();
13910         calibrator.getBiasAsTriad(biasTriad2);
13911         assertEquals(biasTriad1, biasTriad2);
13912         assertEquals(sx, calibrator.getInitialSx(), 0.0);
13913         assertEquals(sy, calibrator.getInitialSy(), 0.0);
13914         assertEquals(sz, calibrator.getInitialSz(), 0.0);
13915         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
13916         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
13917         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
13918         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
13919         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
13920         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
13921         final var bias1 = calibrator.getBias();
13922         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13923         final var bias2 = new double[3];
13924         calibrator.getBias(bias2);
13925         assertArrayEquals(bias1, bias2, 0.0);
13926         final var b1 = calibrator.getBiasAsMatrix();
13927         assertEquals(b1, ba);
13928         final var b2 = new Matrix(3, 1);
13929         calibrator.getBiasAsMatrix(b2);
13930         assertEquals(b1, b2);
13931         final var ma1 = new Matrix(3, 3);
13932         ma1.setSubmatrix(0, 0, 2, 2,
13933                 new double[]{sx, myx, mzx,
13934                         mxy, sy, mzy,
13935                         mxz, myz, sz});
13936         assertEquals(ma1, calibrator.getInitialMa());
13937         final var ma2 = new Matrix(3, 3);
13938         calibrator.getInitialMa(ma2);
13939         assertEquals(ma1, ma2);
13940         assertNull(calibrator.getMeasurements());
13941         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
13942                 calibrator.getMeasurementType());
13943         assertFalse(calibrator.isOrderedMeasurementsRequired());
13944         assertFalse(calibrator.isQualityScoresRequired());
13945         assertFalse(calibrator.isCommonAxisUsed());
13946         assertNull(calibrator.getListener());
13947         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
13948         assertFalse(calibrator.isReady());
13949         assertFalse(calibrator.isRunning());
13950         assertNull(calibrator.getEstimatedMa());
13951         assertNull(calibrator.getEstimatedSx());
13952         assertNull(calibrator.getEstimatedSy());
13953         assertNull(calibrator.getEstimatedSz());
13954         assertNull(calibrator.getEstimatedMxy());
13955         assertNull(calibrator.getEstimatedMxz());
13956         assertNull(calibrator.getEstimatedMyx());
13957         assertNull(calibrator.getEstimatedMyz());
13958         assertNull(calibrator.getEstimatedMzx());
13959         assertNull(calibrator.getEstimatedMzy());
13960         assertNull(calibrator.getEstimatedCovariance());
13961         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
13962         assertNotNull(calibrator.getGroundTruthGravityNorm());
13963         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13964         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13965         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
13966                 ABSOLUTE_ERROR));
13967         final var g = new Acceleration(0.0, AccelerationUnit.G);
13968         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13969         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13970         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
13971 
13972         // Force IllegalArgumentException
13973         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
13974                 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
13975     }
13976 
13977     @Test
13978     void testConstructor126() throws WrongSizeException {
13979         final var ba = generateBa();
13980         final var biasX = ba.getElementAtIndex(0);
13981         final var biasY = ba.getElementAtIndex(1);
13982         final var biasZ = ba.getElementAtIndex(2);
13983 
13984         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13985         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13986         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
13987 
13988         final var ma = generateMaCommonAxis();
13989         final var sx = ma.getElementAt(0, 0);
13990         final var sy = ma.getElementAt(1, 1);
13991         final var sz = ma.getElementAt(2, 2);
13992         final var mxy = ma.getElementAt(0, 1);
13993         final var mxz = ma.getElementAt(0, 2);
13994         final var myx = ma.getElementAt(1, 0);
13995         final var myz = ma.getElementAt(1, 2);
13996         final var mzx = ma.getElementAt(2, 0);
13997         final var mzy = ma.getElementAt(2, 1);
13998 
13999         final var randomizer = new UniformRandomizer();
14000         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14001         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14002         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14003         final var nedPosition = new NEDPosition(latitude, longitude, height);
14004         final var nedVelocity = new NEDVelocity();
14005         final var ecefPosition = new ECEFPosition();
14006         final var ecefVelocity = new ECEFVelocity();
14007         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14008         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14009                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14010         final var gravityNorm = gravity.getNorm();
14011 
14012         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
14013                 mxy, mxz, myx, myz, mzx, mzy, this);
14014 
14015         // check default values
14016         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14017         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14018         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14019         final var bx1 = calibrator.getBiasXAsAcceleration();
14020         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14021         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14022         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14023         calibrator.getBiasXAsAcceleration(bx2);
14024         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14025         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14026         final var by1 = calibrator.getBiasYAsAcceleration();
14027         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14028         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14029         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14030         calibrator.getBiasYAsAcceleration(by2);
14031         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14032         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14033         final var bz1 = calibrator.getBiasZAsAcceleration();
14034         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14035         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14036         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14037         calibrator.getBiasZAsAcceleration(bz2);
14038         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14039         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14040         final var biasTriad1 = calibrator.getBiasAsTriad();
14041         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14042         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14043         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14044         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14045         final var biasTriad2 = new AccelerationTriad();
14046         calibrator.getBiasAsTriad(biasTriad2);
14047         assertEquals(biasTriad1, biasTriad2);
14048         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14049         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14050         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14051         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14052         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14053         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14054         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14055         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14056         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14057         final var bias1 = calibrator.getBias();
14058         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14059         final var bias2 = new double[3];
14060         calibrator.getBias(bias2);
14061         assertArrayEquals(bias1, bias2, 0.0);
14062         final var b1 = calibrator.getBiasAsMatrix();
14063         assertEquals(b1, ba);
14064         final var b2 = new Matrix(3, 1);
14065         calibrator.getBiasAsMatrix(b2);
14066         assertEquals(b1, b2);
14067         final var ma1 = new Matrix(3, 3);
14068         ma1.setSubmatrix(0, 0, 2, 2,
14069                 new double[]{sx, myx, mzx,
14070                         mxy, sy, mzy,
14071                         mxz, myz, sz});
14072         assertEquals(ma1, calibrator.getInitialMa());
14073         final var ma2 = new Matrix(3, 3);
14074         calibrator.getInitialMa(ma2);
14075         assertEquals(ma1, ma2);
14076         assertNull(calibrator.getMeasurements());
14077         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14078                 calibrator.getMeasurementType());
14079         assertFalse(calibrator.isOrderedMeasurementsRequired());
14080         assertFalse(calibrator.isQualityScoresRequired());
14081         assertFalse(calibrator.isCommonAxisUsed());
14082         assertSame(this, calibrator.getListener());
14083         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
14084         assertFalse(calibrator.isReady());
14085         assertFalse(calibrator.isRunning());
14086         assertNull(calibrator.getEstimatedMa());
14087         assertNull(calibrator.getEstimatedSx());
14088         assertNull(calibrator.getEstimatedSy());
14089         assertNull(calibrator.getEstimatedSz());
14090         assertNull(calibrator.getEstimatedMxy());
14091         assertNull(calibrator.getEstimatedMxz());
14092         assertNull(calibrator.getEstimatedMyx());
14093         assertNull(calibrator.getEstimatedMyz());
14094         assertNull(calibrator.getEstimatedMzx());
14095         assertNull(calibrator.getEstimatedMzy());
14096         assertNull(calibrator.getEstimatedCovariance());
14097         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14098         assertNotNull(calibrator.getGroundTruthGravityNorm());
14099         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14100         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14101         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14102                 ABSOLUTE_ERROR));
14103         final var g = new Acceleration(0.0, AccelerationUnit.G);
14104         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14105         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14106         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14107 
14108         // Force IllegalArgumentException
14109         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14110                 -gravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14111     }
14112 
14113     @Test
14114     void testConstructor127() throws WrongSizeException {
14115         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14116 
14117         final var ba = generateBa();
14118         final var biasX = ba.getElementAtIndex(0);
14119         final var biasY = ba.getElementAtIndex(1);
14120         final var biasZ = ba.getElementAtIndex(2);
14121 
14122         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14123         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14124         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14125 
14126         final var ma = generateMaCommonAxis();
14127         final var sx = ma.getElementAt(0, 0);
14128         final var sy = ma.getElementAt(1, 1);
14129         final var sz = ma.getElementAt(2, 2);
14130         final var mxy = ma.getElementAt(0, 1);
14131         final var mxz = ma.getElementAt(0, 2);
14132         final var myx = ma.getElementAt(1, 0);
14133         final var myz = ma.getElementAt(1, 2);
14134         final var mzx = ma.getElementAt(2, 0);
14135         final var mzy = ma.getElementAt(2, 1);
14136 
14137         final var randomizer = new UniformRandomizer();
14138         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14139         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14140         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14141         final var nedPosition = new NEDPosition(latitude, longitude, height);
14142         final var nedVelocity = new NEDVelocity();
14143         final var ecefPosition = new ECEFPosition();
14144         final var ecefVelocity = new ECEFVelocity();
14145         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14146         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14147                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14148         final var gravityNorm = gravity.getNorm();
14149 
14150         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
14151                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14152 
14153         // check default values
14154         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14155         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14156         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14157         final var bx1 = calibrator.getBiasXAsAcceleration();
14158         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14159         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14160         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14161         calibrator.getBiasXAsAcceleration(bx2);
14162         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14163         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14164         final var by1 = calibrator.getBiasYAsAcceleration();
14165         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14166         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14167         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14168         calibrator.getBiasYAsAcceleration(by2);
14169         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14170         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14171         final var bz1 = calibrator.getBiasZAsAcceleration();
14172         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14173         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14174         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14175         calibrator.getBiasZAsAcceleration(bz2);
14176         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14177         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14178         final var biasTriad1 = calibrator.getBiasAsTriad();
14179         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14180         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14181         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14182         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14183         final var biasTriad2 = new AccelerationTriad();
14184         calibrator.getBiasAsTriad(biasTriad2);
14185         assertEquals(biasTriad1, biasTriad2);
14186         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14187         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14188         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14189         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14190         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14191         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14192         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14193         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14194         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14195         final var bias1 = calibrator.getBias();
14196         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14197         final var bias2 = new double[3];
14198         calibrator.getBias(bias2);
14199         assertArrayEquals(bias1, bias2, 0.0);
14200         final var b1 = calibrator.getBiasAsMatrix();
14201         assertEquals(b1, ba);
14202         final var b2 = new Matrix(3, 1);
14203         calibrator.getBiasAsMatrix(b2);
14204         assertEquals(b1, b2);
14205         final var ma1 = new Matrix(3, 3);
14206         ma1.setSubmatrix(0, 0, 2, 2,
14207                 new double[]{sx, myx, mzx,
14208                         mxy, sy, mzy,
14209                         mxz, myz, sz});
14210         assertEquals(ma1, calibrator.getInitialMa());
14211         final var ma2 = new Matrix(3, 3);
14212         calibrator.getInitialMa(ma2);
14213         assertEquals(ma1, ma2);
14214         assertSame(measurements, calibrator.getMeasurements());
14215         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14216                 calibrator.getMeasurementType());
14217         assertFalse(calibrator.isOrderedMeasurementsRequired());
14218         assertFalse(calibrator.isQualityScoresRequired());
14219         assertFalse(calibrator.isCommonAxisUsed());
14220         assertNull(calibrator.getListener());
14221         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
14222         assertFalse(calibrator.isReady());
14223         assertFalse(calibrator.isRunning());
14224         assertNull(calibrator.getEstimatedMa());
14225         assertNull(calibrator.getEstimatedSx());
14226         assertNull(calibrator.getEstimatedSy());
14227         assertNull(calibrator.getEstimatedSz());
14228         assertNull(calibrator.getEstimatedMxy());
14229         assertNull(calibrator.getEstimatedMxz());
14230         assertNull(calibrator.getEstimatedMyx());
14231         assertNull(calibrator.getEstimatedMyz());
14232         assertNull(calibrator.getEstimatedMzx());
14233         assertNull(calibrator.getEstimatedMzy());
14234         assertNull(calibrator.getEstimatedCovariance());
14235         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14236         assertNotNull(calibrator.getGroundTruthGravityNorm());
14237         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14238         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14239         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14240                 ABSOLUTE_ERROR));
14241         final var g = new Acceleration(0.0, AccelerationUnit.G);
14242         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14243         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14244         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14245 
14246         // Force IllegalArgumentException
14247         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14248                 -gravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
14249     }
14250 
14251     @Test
14252     void testConstructor128() throws WrongSizeException {
14253         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14254 
14255         final var ba = generateBa();
14256         final var biasX = ba.getElementAtIndex(0);
14257         final var biasY = ba.getElementAtIndex(1);
14258         final var biasZ = ba.getElementAtIndex(2);
14259 
14260         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14261         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14262         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14263 
14264         final var ma = generateMaCommonAxis();
14265         final var sx = ma.getElementAt(0, 0);
14266         final var sy = ma.getElementAt(1, 1);
14267         final var sz = ma.getElementAt(2, 2);
14268         final var mxy = ma.getElementAt(0, 1);
14269         final var mxz = ma.getElementAt(0, 2);
14270         final var myx = ma.getElementAt(1, 0);
14271         final var myz = ma.getElementAt(1, 2);
14272         final var mzx = ma.getElementAt(2, 0);
14273         final var mzy = ma.getElementAt(2, 1);
14274 
14275         final var randomizer = new UniformRandomizer();
14276         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14277         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14278         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14279         final var nedPosition = new NEDPosition(latitude, longitude, height);
14280         final var nedVelocity = new NEDVelocity();
14281         final var ecefPosition = new ECEFPosition();
14282         final var ecefVelocity = new ECEFVelocity();
14283         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14284         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14285                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14286         final var gravityNorm = gravity.getNorm();
14287 
14288         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
14289                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
14290 
14291         // check default values
14292         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14293         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14294         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14295         final var bx1 = calibrator.getBiasXAsAcceleration();
14296         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14297         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14298         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14299         calibrator.getBiasXAsAcceleration(bx2);
14300         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14301         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14302         final var by1 = calibrator.getBiasYAsAcceleration();
14303         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14304         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14305         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14306         calibrator.getBiasYAsAcceleration(by2);
14307         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14308         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14309         final var bz1 = calibrator.getBiasZAsAcceleration();
14310         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14311         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14312         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14313         calibrator.getBiasZAsAcceleration(bz2);
14314         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14315         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14316         final var biasTriad1 = calibrator.getBiasAsTriad();
14317         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14318         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14319         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14320         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14321         final var biasTriad2 = new AccelerationTriad();
14322         calibrator.getBiasAsTriad(biasTriad2);
14323         assertEquals(biasTriad1, biasTriad2);
14324         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14325         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14326         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14327         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14328         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14329         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14330         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14331         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14332         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14333         final var bias1 = calibrator.getBias();
14334         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14335         final var bias2 = new double[3];
14336         calibrator.getBias(bias2);
14337         assertArrayEquals(bias1, bias2, 0.0);
14338         final var b1 = calibrator.getBiasAsMatrix();
14339         assertEquals(b1, ba);
14340         final var b2 = new Matrix(3, 1);
14341         calibrator.getBiasAsMatrix(b2);
14342         assertEquals(b1, b2);
14343         final var ma1 = new Matrix(3, 3);
14344         ma1.setSubmatrix(0, 0, 2, 2,
14345                 new double[]{sx, myx, mzx,
14346                         mxy, sy, mzy,
14347                         mxz, myz, sz});
14348         assertEquals(ma1, calibrator.getInitialMa());
14349         final var ma2 = new Matrix(3, 3);
14350         calibrator.getInitialMa(ma2);
14351         assertEquals(ma1, ma2);
14352         assertSame(measurements, calibrator.getMeasurements());
14353         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14354                 calibrator.getMeasurementType());
14355         assertFalse(calibrator.isOrderedMeasurementsRequired());
14356         assertFalse(calibrator.isQualityScoresRequired());
14357         assertFalse(calibrator.isCommonAxisUsed());
14358         assertSame(this, calibrator.getListener());
14359         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
14360         assertFalse(calibrator.isReady());
14361         assertFalse(calibrator.isRunning());
14362         assertNull(calibrator.getEstimatedMa());
14363         assertNull(calibrator.getEstimatedSx());
14364         assertNull(calibrator.getEstimatedSy());
14365         assertNull(calibrator.getEstimatedSz());
14366         assertNull(calibrator.getEstimatedMxy());
14367         assertNull(calibrator.getEstimatedMxz());
14368         assertNull(calibrator.getEstimatedMyx());
14369         assertNull(calibrator.getEstimatedMyz());
14370         assertNull(calibrator.getEstimatedMzx());
14371         assertNull(calibrator.getEstimatedMzy());
14372         assertNull(calibrator.getEstimatedCovariance());
14373         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14374         assertNotNull(calibrator.getGroundTruthGravityNorm());
14375         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14376         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14377         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(
14378                 gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14379         final var g = new Acceleration(0.0, AccelerationUnit.G);
14380         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14381         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14382         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14383 
14384         // Force IllegalArgumentException
14385         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14386                 -gravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14387     }
14388 
14389     @Test
14390     void testConstructor129() throws WrongSizeException {
14391         final var ba = generateBa();
14392         final var biasX = ba.getElementAtIndex(0);
14393         final var biasY = ba.getElementAtIndex(1);
14394         final var biasZ = ba.getElementAtIndex(2);
14395 
14396         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14397         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14398         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14399 
14400         final var ma = generateMaCommonAxis();
14401         final var sx = ma.getElementAt(0, 0);
14402         final var sy = ma.getElementAt(1, 1);
14403         final var sz = ma.getElementAt(2, 2);
14404         final var mxy = ma.getElementAt(0, 1);
14405         final var mxz = ma.getElementAt(0, 2);
14406         final var myx = ma.getElementAt(1, 0);
14407         final var myz = ma.getElementAt(1, 2);
14408         final var mzx = ma.getElementAt(2, 0);
14409         final var mzy = ma.getElementAt(2, 1);
14410 
14411         final var randomizer = new UniformRandomizer();
14412         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14413         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14414         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14415         final var nedPosition = new NEDPosition(latitude, longitude, height);
14416         final var nedVelocity = new NEDVelocity();
14417         final var ecefPosition = new ECEFPosition();
14418         final var ecefVelocity = new ECEFVelocity();
14419         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14420         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14421                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14422         final var gravityNorm = gravity.getNorm();
14423 
14424         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
14425                 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14426 
14427         // check default values
14428         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14429         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14430         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14431         final var bx1 = calibrator.getBiasXAsAcceleration();
14432         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14433         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14434         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14435         calibrator.getBiasXAsAcceleration(bx2);
14436         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14437         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14438         final var by1 = calibrator.getBiasYAsAcceleration();
14439         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14440         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14441         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14442         calibrator.getBiasYAsAcceleration(by2);
14443         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14444         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14445         final var bz1 = calibrator.getBiasZAsAcceleration();
14446         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14447         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14448         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14449         calibrator.getBiasZAsAcceleration(bz2);
14450         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14451         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14452         final var biasTriad1 = calibrator.getBiasAsTriad();
14453         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14454         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14455         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14456         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14457         final var biasTriad2 = new AccelerationTriad();
14458         calibrator.getBiasAsTriad(biasTriad2);
14459         assertEquals(biasTriad1, biasTriad2);
14460         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14461         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14462         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14463         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14464         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14465         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14466         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14467         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14468         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14469         final var bias1 = calibrator.getBias();
14470         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14471         final var bias2 = new double[3];
14472         calibrator.getBias(bias2);
14473         assertArrayEquals(bias1, bias2, 0.0);
14474         final var b1 = calibrator.getBiasAsMatrix();
14475         assertEquals(b1, ba);
14476         final var b2 = new Matrix(3, 1);
14477         calibrator.getBiasAsMatrix(b2);
14478         assertEquals(b1, b2);
14479         final var ma1 = new Matrix(3, 3);
14480         ma1.setSubmatrix(0, 0, 2, 2,
14481                 new double[]{sx, myx, mzx,
14482                         mxy, sy, mzy,
14483                         mxz, myz, sz});
14484         assertEquals(calibrator.getInitialMa(), ma1);
14485         final var ma2 = new Matrix(3, 3);
14486         calibrator.getInitialMa(ma2);
14487         assertEquals(ma1, ma2);
14488         assertNull(calibrator.getMeasurements());
14489         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14490                 calibrator.getMeasurementType());
14491         assertFalse(calibrator.isOrderedMeasurementsRequired());
14492         assertFalse(calibrator.isQualityScoresRequired());
14493         assertTrue(calibrator.isCommonAxisUsed());
14494         assertNull(calibrator.getListener());
14495         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14496         assertFalse(calibrator.isReady());
14497         assertFalse(calibrator.isRunning());
14498         assertNull(calibrator.getEstimatedMa());
14499         assertNull(calibrator.getEstimatedSx());
14500         assertNull(calibrator.getEstimatedSy());
14501         assertNull(calibrator.getEstimatedSz());
14502         assertNull(calibrator.getEstimatedMxy());
14503         assertNull(calibrator.getEstimatedMxz());
14504         assertNull(calibrator.getEstimatedMyx());
14505         assertNull(calibrator.getEstimatedMyz());
14506         assertNull(calibrator.getEstimatedMzx());
14507         assertNull(calibrator.getEstimatedMzy());
14508         assertNull(calibrator.getEstimatedCovariance());
14509         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14510         assertNotNull(calibrator.getGroundTruthGravityNorm());
14511         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14512         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14513         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14514                 ABSOLUTE_ERROR));
14515         final var g = new Acceleration(0.0, AccelerationUnit.G);
14516         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14517         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14518         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14519 
14520         // Force IllegalArgumentException
14521         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14522                 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
14523     }
14524 
14525     @Test
14526     void testConstructor130() throws WrongSizeException {
14527         final var ba = generateBa();
14528         final var biasX = ba.getElementAtIndex(0);
14529         final var biasY = ba.getElementAtIndex(1);
14530         final var biasZ = ba.getElementAtIndex(2);
14531 
14532         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14533         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14534         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14535 
14536         final var ma = generateMaCommonAxis();
14537         final var sx = ma.getElementAt(0, 0);
14538         final var sy = ma.getElementAt(1, 1);
14539         final var sz = ma.getElementAt(2, 2);
14540         final var mxy = ma.getElementAt(0, 1);
14541         final var mxz = ma.getElementAt(0, 2);
14542         final var myx = ma.getElementAt(1, 0);
14543         final var myz = ma.getElementAt(1, 2);
14544         final var mzx = ma.getElementAt(2, 0);
14545         final var mzy = ma.getElementAt(2, 1);
14546 
14547         final var randomizer = new UniformRandomizer();
14548         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14549         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14550         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14551         final var nedPosition = new NEDPosition(latitude, longitude, height);
14552         final var nedVelocity = new NEDVelocity();
14553         final var ecefPosition = new ECEFPosition();
14554         final var ecefVelocity = new ECEFVelocity();
14555         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14556         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14557                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14558         final var gravityNorm = gravity.getNorm();
14559 
14560         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
14561                 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
14562 
14563         // check default values
14564         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14565         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14566         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14567         final var bx1 = calibrator.getBiasXAsAcceleration();
14568         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14569         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14570         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14571         calibrator.getBiasXAsAcceleration(bx2);
14572         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14573         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14574         final var by1 = calibrator.getBiasYAsAcceleration();
14575         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14576         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14577         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14578         calibrator.getBiasYAsAcceleration(by2);
14579         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14580         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14581         final var bz1 = calibrator.getBiasZAsAcceleration();
14582         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14583         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14584         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14585         calibrator.getBiasZAsAcceleration(bz2);
14586         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14587         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14588         final var biasTriad1 = calibrator.getBiasAsTriad();
14589         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14590         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14591         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14592         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14593         final var biasTriad2 = new AccelerationTriad();
14594         calibrator.getBiasAsTriad(biasTriad2);
14595         assertEquals(biasTriad1, biasTriad2);
14596         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14597         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14598         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14599         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14600         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14601         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14602         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14603         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14604         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14605         final var bias1 = calibrator.getBias();
14606         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14607         final var bias2 = new double[3];
14608         calibrator.getBias(bias2);
14609         assertArrayEquals(bias1, bias2, 0.0);
14610         final var b1 = calibrator.getBiasAsMatrix();
14611         assertEquals(b1, ba);
14612         final var b2 = new Matrix(3, 1);
14613         calibrator.getBiasAsMatrix(b2);
14614         assertEquals(b1, b2);
14615         final var ma1 = new Matrix(3, 3);
14616         ma1.setSubmatrix(0, 0, 2, 2,
14617                 new double[]{sx, myx, mzx,
14618                         mxy, sy, mzy,
14619                         mxz, myz, sz});
14620         assertEquals(ma1, calibrator.getInitialMa());
14621         final var ma2 = new Matrix(3, 3);
14622         calibrator.getInitialMa(ma2);
14623         assertEquals(ma1, ma2);
14624         assertNull(calibrator.getMeasurements());
14625         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14626                 calibrator.getMeasurementType());
14627         assertFalse(calibrator.isOrderedMeasurementsRequired());
14628         assertFalse(calibrator.isQualityScoresRequired());
14629         assertTrue(calibrator.isCommonAxisUsed());
14630         assertSame(this, calibrator.getListener());
14631         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14632         assertFalse(calibrator.isReady());
14633         assertFalse(calibrator.isRunning());
14634         assertNull(calibrator.getEstimatedMa());
14635         assertNull(calibrator.getEstimatedSx());
14636         assertNull(calibrator.getEstimatedSy());
14637         assertNull(calibrator.getEstimatedSz());
14638         assertNull(calibrator.getEstimatedMxy());
14639         assertNull(calibrator.getEstimatedMxz());
14640         assertNull(calibrator.getEstimatedMyx());
14641         assertNull(calibrator.getEstimatedMyz());
14642         assertNull(calibrator.getEstimatedMzx());
14643         assertNull(calibrator.getEstimatedMzy());
14644         assertNull(calibrator.getEstimatedCovariance());
14645         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14646         assertNotNull(calibrator.getGroundTruthGravityNorm());
14647         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14648         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14649         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14650                 ABSOLUTE_ERROR));
14651         final var g = new Acceleration(0.0, AccelerationUnit.G);
14652         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14653         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14654         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14655 
14656         // Force IllegalArgumentException
14657         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14658                 -gravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14659     }
14660 
14661     @Test
14662     void testConstructor131() throws WrongSizeException {
14663         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14664 
14665         final var ba = generateBa();
14666         final var biasX = ba.getElementAtIndex(0);
14667         final var biasY = ba.getElementAtIndex(1);
14668         final var biasZ = ba.getElementAtIndex(2);
14669 
14670         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14671         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14672         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14673 
14674         final var ma = generateMaCommonAxis();
14675         final var sx = ma.getElementAt(0, 0);
14676         final var sy = ma.getElementAt(1, 1);
14677         final var sz = ma.getElementAt(2, 2);
14678         final var mxy = ma.getElementAt(0, 1);
14679         final var mxz = ma.getElementAt(0, 2);
14680         final var myx = ma.getElementAt(1, 0);
14681         final var myz = ma.getElementAt(1, 2);
14682         final var mzx = ma.getElementAt(2, 0);
14683         final var mzy = ma.getElementAt(2, 1);
14684 
14685         final var randomizer = new UniformRandomizer();
14686         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14687         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14688         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14689         final var nedPosition = new NEDPosition(latitude, longitude, height);
14690         final var nedVelocity = new NEDVelocity();
14691         final var ecefPosition = new ECEFPosition();
14692         final var ecefVelocity = new ECEFVelocity();
14693         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14694         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14695                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14696         final var gravityNorm = gravity.getNorm();
14697 
14698         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
14699                 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
14700 
14701         // check default values
14702         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14703         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14704         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14705         final var bx1 = calibrator.getBiasXAsAcceleration();
14706         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14707         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14708         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14709         calibrator.getBiasXAsAcceleration(bx2);
14710         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14711         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14712         final var by1 = calibrator.getBiasYAsAcceleration();
14713         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14714         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14715         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14716         calibrator.getBiasYAsAcceleration(by2);
14717         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14718         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14719         final var bz1 = calibrator.getBiasZAsAcceleration();
14720         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14721         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14722         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14723         calibrator.getBiasZAsAcceleration(bz2);
14724         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14725         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14726         final var biasTriad1 = calibrator.getBiasAsTriad();
14727         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14728         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14729         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14730         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14731         final var biasTriad2 = new AccelerationTriad();
14732         calibrator.getBiasAsTriad(biasTriad2);
14733         assertEquals(biasTriad1, biasTriad2);
14734         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14735         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14736         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14737         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14738         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14739         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14740         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14741         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14742         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14743         final var bias1 = calibrator.getBias();
14744         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14745         final var bias2 = new double[3];
14746         calibrator.getBias(bias2);
14747         assertArrayEquals(bias1, bias2, 0.0);
14748         final var b1 = calibrator.getBiasAsMatrix();
14749         assertEquals(b1, ba);
14750         final var b2 = new Matrix(3, 1);
14751         calibrator.getBiasAsMatrix(b2);
14752         assertEquals(b1, b2);
14753         final var ma1 = new Matrix(3, 3);
14754         ma1.setSubmatrix(0, 0, 2, 2,
14755                 new double[]{sx, myx, mzx,
14756                         mxy, sy, mzy,
14757                         mxz, myz, sz});
14758         assertEquals(ma1, calibrator.getInitialMa());
14759         final var ma2 = new Matrix(3, 3);
14760         calibrator.getInitialMa(ma2);
14761         assertEquals(ma1, ma2);
14762         assertSame(measurements, calibrator.getMeasurements());
14763         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14764                 calibrator.getMeasurementType());
14765         assertFalse(calibrator.isOrderedMeasurementsRequired());
14766         assertFalse(calibrator.isQualityScoresRequired());
14767         assertTrue(calibrator.isCommonAxisUsed());
14768         assertNull(calibrator.getListener());
14769         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14770         assertFalse(calibrator.isReady());
14771         assertFalse(calibrator.isRunning());
14772         assertNull(calibrator.getEstimatedMa());
14773         assertNull(calibrator.getEstimatedSx());
14774         assertNull(calibrator.getEstimatedSy());
14775         assertNull(calibrator.getEstimatedSz());
14776         assertNull(calibrator.getEstimatedMxy());
14777         assertNull(calibrator.getEstimatedMxz());
14778         assertNull(calibrator.getEstimatedMyx());
14779         assertNull(calibrator.getEstimatedMyz());
14780         assertNull(calibrator.getEstimatedMzx());
14781         assertNull(calibrator.getEstimatedMzy());
14782         assertNull(calibrator.getEstimatedCovariance());
14783         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14784         assertNotNull(calibrator.getGroundTruthGravityNorm());
14785         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14786         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14787         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14788                 ABSOLUTE_ERROR));
14789         final var g = new Acceleration(0.0, AccelerationUnit.G);
14790         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14791         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14792         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14793 
14794         // Force IllegalArgumentException
14795         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14796                 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
14797     }
14798 
14799     @Test
14800     void testConstructor132() throws WrongSizeException {
14801         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
14802 
14803         final var ba = generateBa();
14804         final var biasX = ba.getElementAtIndex(0);
14805         final var biasY = ba.getElementAtIndex(1);
14806         final var biasZ = ba.getElementAtIndex(2);
14807 
14808         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14809         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14810         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
14811 
14812         final var ma = generateMaCommonAxis();
14813         final var sx = ma.getElementAt(0, 0);
14814         final var sy = ma.getElementAt(1, 1);
14815         final var sz = ma.getElementAt(2, 2);
14816         final var mxy = ma.getElementAt(0, 1);
14817         final var mxz = ma.getElementAt(0, 2);
14818         final var myx = ma.getElementAt(1, 0);
14819         final var myz = ma.getElementAt(1, 2);
14820         final var mzx = ma.getElementAt(2, 0);
14821         final var mzy = ma.getElementAt(2, 1);
14822 
14823         final var randomizer = new UniformRandomizer();
14824         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14825         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14826         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14827         final var nedPosition = new NEDPosition(latitude, longitude, height);
14828         final var nedVelocity = new NEDVelocity();
14829         final var ecefPosition = new ECEFPosition();
14830         final var ecefVelocity = new ECEFVelocity();
14831         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14832         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14833                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14834         final var gravityNorm = gravity.getNorm();
14835 
14836         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
14837                 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
14838 
14839         // check default values
14840         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14841         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14842         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14843         final var bx1 = calibrator.getBiasXAsAcceleration();
14844         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14845         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14846         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14847         calibrator.getBiasXAsAcceleration(bx2);
14848         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14849         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14850         final var by1 = calibrator.getBiasYAsAcceleration();
14851         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14852         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14853         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14854         calibrator.getBiasYAsAcceleration(by2);
14855         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14856         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14857         final var bz1 = calibrator.getBiasZAsAcceleration();
14858         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14859         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14860         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14861         calibrator.getBiasZAsAcceleration(bz2);
14862         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14863         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14864         final var biasTriad1 = calibrator.getBiasAsTriad();
14865         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14866         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14867         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14868         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14869         final var biasTriad2 = new AccelerationTriad();
14870         calibrator.getBiasAsTriad(biasTriad2);
14871         assertEquals(biasTriad1, biasTriad2);
14872         assertEquals(sx, calibrator.getInitialSx(), 0.0);
14873         assertEquals(sy, calibrator.getInitialSy(), 0.0);
14874         assertEquals(sz, calibrator.getInitialSz(), 0.0);
14875         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
14876         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
14877         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
14878         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
14879         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
14880         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
14881         final var bias1 = calibrator.getBias();
14882         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14883         final var bias2 = new double[3];
14884         calibrator.getBias(bias2);
14885         assertArrayEquals(bias1, bias2, 0.0);
14886         final var b1 = calibrator.getBiasAsMatrix();
14887         assertEquals(b1, ba);
14888         final var b2 = new Matrix(3, 1);
14889         calibrator.getBiasAsMatrix(b2);
14890         assertEquals(b1, b2);
14891         final var ma1 = new Matrix(3, 3);
14892         ma1.setSubmatrix(0, 0, 2, 2,
14893                 new double[]{sx, myx, mzx,
14894                         mxy, sy, mzy,
14895                         mxz, myz, sz});
14896         assertEquals(ma1, calibrator.getInitialMa());
14897         final var ma2 = new Matrix(3, 3);
14898         calibrator.getInitialMa(ma2);
14899         assertEquals(ma1, ma2);
14900         assertSame(measurements, calibrator.getMeasurements());
14901         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
14902                 calibrator.getMeasurementType());
14903         assertFalse(calibrator.isOrderedMeasurementsRequired());
14904         assertFalse(calibrator.isQualityScoresRequired());
14905         assertTrue(calibrator.isCommonAxisUsed());
14906         assertSame(this, calibrator.getListener());
14907         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
14908         assertFalse(calibrator.isReady());
14909         assertFalse(calibrator.isRunning());
14910         assertNull(calibrator.getEstimatedMa());
14911         assertNull(calibrator.getEstimatedSx());
14912         assertNull(calibrator.getEstimatedSy());
14913         assertNull(calibrator.getEstimatedSz());
14914         assertNull(calibrator.getEstimatedMxy());
14915         assertNull(calibrator.getEstimatedMxz());
14916         assertNull(calibrator.getEstimatedMyx());
14917         assertNull(calibrator.getEstimatedMyz());
14918         assertNull(calibrator.getEstimatedMzx());
14919         assertNull(calibrator.getEstimatedMzy());
14920         assertNull(calibrator.getEstimatedCovariance());
14921         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
14922         assertNotNull(calibrator.getGroundTruthGravityNorm());
14923         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14924         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14925         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
14926                 ABSOLUTE_ERROR));
14927         final var g = new Acceleration(0.0, AccelerationUnit.G);
14928         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14929         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14930         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
14931 
14932         // Force IllegalArgumentException
14933         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
14934                 -gravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
14935     }
14936 
14937     @Test
14938     void testConstructor133() throws WrongSizeException {
14939         final var ba = generateBa();
14940         final var bias = ba.getBuffer();
14941         final var biasX = ba.getElementAtIndex(0);
14942         final var biasY = ba.getElementAtIndex(1);
14943         final var biasZ = ba.getElementAtIndex(2);
14944 
14945         final var randomizer = new UniformRandomizer();
14946         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
14947         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14948         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14949         final var nedPosition = new NEDPosition(latitude, longitude, height);
14950         final var nedVelocity = new NEDVelocity();
14951         final var ecefPosition = new ECEFPosition();
14952         final var ecefVelocity = new ECEFVelocity();
14953         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
14954         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14955                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14956         final var gravityNorm = gravity.getNorm();
14957 
14958         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias);
14959 
14960         // check default values
14961         assertEquals(biasX, calibrator.getBiasX(), 0.0);
14962         assertEquals(biasY, calibrator.getBiasY(), 0.0);
14963         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
14964         final var bx1 = calibrator.getBiasXAsAcceleration();
14965         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
14966         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
14967         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14968         calibrator.getBiasXAsAcceleration(bx2);
14969         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
14970         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
14971         final var by1 = calibrator.getBiasYAsAcceleration();
14972         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
14973         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
14974         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14975         calibrator.getBiasYAsAcceleration(by2);
14976         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
14977         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
14978         final var bz1 = calibrator.getBiasZAsAcceleration();
14979         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
14980         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
14981         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
14982         calibrator.getBiasZAsAcceleration(bz2);
14983         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
14984         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
14985         final var biasTriad1 = calibrator.getBiasAsTriad();
14986         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
14987         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
14988         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
14989         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
14990         final var biasTriad2 = new AccelerationTriad();
14991         calibrator.getBiasAsTriad(biasTriad2);
14992         assertEquals(biasTriad1, biasTriad2);
14993         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
14994         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
14995         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
14996         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
14997         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
14998         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
14999         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15000         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15001         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15002         final var bias1 = calibrator.getBias();
15003         assertArrayEquals(bias1, bias, 0.0);
15004         final var bias2 = new double[3];
15005         calibrator.getBias(bias2);
15006         assertArrayEquals(bias1, bias2, 0.0);
15007         final var b1 = calibrator.getBiasAsMatrix();
15008         assertEquals(b1, ba);
15009         final var b2 = new Matrix(3, 1);
15010         calibrator.getBiasAsMatrix(b2);
15011         assertEquals(b1, b2);
15012         final var ma1 = calibrator.getInitialMa();
15013         assertEquals(new Matrix(3, 3), ma1);
15014         final var ma2 = new Matrix(3, 3);
15015         calibrator.getInitialMa(ma2);
15016         assertEquals(ma1, ma2);
15017         assertNull(calibrator.getMeasurements());
15018         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15019                 calibrator.getMeasurementType());
15020         assertFalse(calibrator.isOrderedMeasurementsRequired());
15021         assertFalse(calibrator.isQualityScoresRequired());
15022         assertFalse(calibrator.isCommonAxisUsed());
15023         assertNull(calibrator.getListener());
15024         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15025         assertFalse(calibrator.isReady());
15026         assertFalse(calibrator.isRunning());
15027         assertNull(calibrator.getEstimatedMa());
15028         assertNull(calibrator.getEstimatedSx());
15029         assertNull(calibrator.getEstimatedSy());
15030         assertNull(calibrator.getEstimatedSz());
15031         assertNull(calibrator.getEstimatedMxy());
15032         assertNull(calibrator.getEstimatedMxz());
15033         assertNull(calibrator.getEstimatedMyx());
15034         assertNull(calibrator.getEstimatedMyz());
15035         assertNull(calibrator.getEstimatedMzx());
15036         assertNull(calibrator.getEstimatedMzy());
15037         assertNull(calibrator.getEstimatedCovariance());
15038         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15039         assertNotNull(calibrator.getGroundTruthGravityNorm());
15040         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15041         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15042         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15043                 ABSOLUTE_ERROR));
15044         final var g = new Acceleration(0.0, AccelerationUnit.G);
15045         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15046         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15047         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15048 
15049         // Force IllegalArgumentException
15050         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15051                 -gravityNorm, bias));
15052         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15053                 gravityNorm, new double[1]));
15054     }
15055 
15056     @Test
15057     void testConstructor134() throws WrongSizeException {
15058         final var ba = generateBa();
15059         final var bias = ba.getBuffer();
15060         final var biasX = ba.getElementAtIndex(0);
15061         final var biasY = ba.getElementAtIndex(1);
15062         final var biasZ = ba.getElementAtIndex(2);
15063 
15064         final var randomizer = new UniformRandomizer();
15065         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15066         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15067         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15068         final var nedPosition = new NEDPosition(latitude, longitude, height);
15069         final var nedVelocity = new NEDVelocity();
15070         final var ecefPosition = new ECEFPosition();
15071         final var ecefVelocity = new ECEFVelocity();
15072         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15073         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15074                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15075         final var gravityNorm = gravity.getNorm();
15076 
15077         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias, this);
15078 
15079         // check default values
15080         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15081         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15082         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15083         final var bx1 = calibrator.getBiasXAsAcceleration();
15084         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15085         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15086         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15087         calibrator.getBiasXAsAcceleration(bx2);
15088         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15089         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15090         final var by1 = calibrator.getBiasYAsAcceleration();
15091         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15092         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15093         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15094         calibrator.getBiasYAsAcceleration(by2);
15095         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15096         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15097         final var bz1 = calibrator.getBiasZAsAcceleration();
15098         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15099         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15100         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15101         calibrator.getBiasZAsAcceleration(bz2);
15102         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15103         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15104         final var biasTriad1 = calibrator.getBiasAsTriad();
15105         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15106         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15107         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15108         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15109         final var biasTriad2 = new AccelerationTriad();
15110         calibrator.getBiasAsTriad(biasTriad2);
15111         assertEquals(biasTriad1, biasTriad2);
15112         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15113         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15114         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15115         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15116         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15117         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15118         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15119         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15120         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15121         final var bias1 = calibrator.getBias();
15122         assertArrayEquals(bias1, bias, 0.0);
15123         final var bias2 = new double[3];
15124         calibrator.getBias(bias2);
15125         assertArrayEquals(bias1, bias2, 0.0);
15126         final var b1 = calibrator.getBiasAsMatrix();
15127         assertEquals(b1, ba);
15128         final var b2 = new Matrix(3, 1);
15129         calibrator.getBiasAsMatrix(b2);
15130         assertEquals(b1, b2);
15131         final var ma1 = calibrator.getInitialMa();
15132         assertEquals(new Matrix(3, 3), ma1);
15133         final var ma2 = new Matrix(3, 3);
15134         calibrator.getInitialMa(ma2);
15135         assertEquals(ma1, ma2);
15136         assertNull(calibrator.getMeasurements());
15137         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15138                 calibrator.getMeasurementType());
15139         assertFalse(calibrator.isOrderedMeasurementsRequired());
15140         assertFalse(calibrator.isQualityScoresRequired());
15141         assertFalse(calibrator.isCommonAxisUsed());
15142         assertSame(this, calibrator.getListener());
15143         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15144         assertFalse(calibrator.isReady());
15145         assertFalse(calibrator.isRunning());
15146         assertNull(calibrator.getEstimatedMa());
15147         assertNull(calibrator.getEstimatedSx());
15148         assertNull(calibrator.getEstimatedSy());
15149         assertNull(calibrator.getEstimatedSz());
15150         assertNull(calibrator.getEstimatedMxy());
15151         assertNull(calibrator.getEstimatedMxz());
15152         assertNull(calibrator.getEstimatedMyx());
15153         assertNull(calibrator.getEstimatedMyz());
15154         assertNull(calibrator.getEstimatedMzx());
15155         assertNull(calibrator.getEstimatedMzy());
15156         assertNull(calibrator.getEstimatedCovariance());
15157         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15158         assertNotNull(calibrator.getGroundTruthGravityNorm());
15159         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15160         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15161         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15162                 ABSOLUTE_ERROR));
15163         final var g = new Acceleration(0.0, AccelerationUnit.G);
15164         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15165         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15166         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15167 
15168         // Force IllegalArgumentException
15169         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15170                 -gravityNorm, bias, this));
15171         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15172                 gravityNorm, new double[1], this));
15173     }
15174 
15175     @Test
15176     void testConstructor135() throws WrongSizeException {
15177         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15178 
15179         final var ba = generateBa();
15180         final var bias = ba.getBuffer();
15181         final var biasX = ba.getElementAtIndex(0);
15182         final var biasY = ba.getElementAtIndex(1);
15183         final var biasZ = ba.getElementAtIndex(2);
15184 
15185         final var randomizer = new UniformRandomizer();
15186         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15187         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15188         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15189         final var nedPosition = new NEDPosition(latitude, longitude, height);
15190         final var nedVelocity = new NEDVelocity();
15191         final var ecefPosition = new ECEFPosition();
15192         final var ecefVelocity = new ECEFVelocity();
15193         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15194         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15195                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15196         final var gravityNorm = gravity.getNorm();
15197 
15198         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias);
15199 
15200         // check default values
15201         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15202         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15203         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15204         final var bx1 = calibrator.getBiasXAsAcceleration();
15205         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15206         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15207         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15208         calibrator.getBiasXAsAcceleration(bx2);
15209         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15210         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15211         final var by1 = calibrator.getBiasYAsAcceleration();
15212         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15213         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15214         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15215         calibrator.getBiasYAsAcceleration(by2);
15216         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15217         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15218         final var bz1 = calibrator.getBiasZAsAcceleration();
15219         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15220         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15221         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15222         calibrator.getBiasZAsAcceleration(bz2);
15223         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15224         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15225         final var biasTriad1 = calibrator.getBiasAsTriad();
15226         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15227         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15228         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15229         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15230         final var biasTriad2 = new AccelerationTriad();
15231         calibrator.getBiasAsTriad(biasTriad2);
15232         assertEquals(biasTriad1, biasTriad2);
15233         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15234         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15235         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15236         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15237         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15238         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15239         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15240         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15241         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15242         final var bias1 = calibrator.getBias();
15243         assertArrayEquals(bias1, bias, 0.0);
15244         final var bias2 = new double[3];
15245         calibrator.getBias(bias2);
15246         assertArrayEquals(bias1, bias2, 0.0);
15247         final var b1 = calibrator.getBiasAsMatrix();
15248         assertEquals(b1, ba);
15249         final var b2 = new Matrix(3, 1);
15250         calibrator.getBiasAsMatrix(b2);
15251         assertEquals(b1, b2);
15252         final var ma1 = calibrator.getInitialMa();
15253         assertEquals(new Matrix(3, 3), ma1);
15254         final var ma2 = new Matrix(3, 3);
15255         calibrator.getInitialMa(ma2);
15256         assertEquals(ma1, ma2);
15257         assertSame(measurements, calibrator.getMeasurements());
15258         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15259                 calibrator.getMeasurementType());
15260         assertFalse(calibrator.isOrderedMeasurementsRequired());
15261         assertFalse(calibrator.isQualityScoresRequired());
15262         assertFalse(calibrator.isCommonAxisUsed());
15263         assertNull(calibrator.getListener());
15264         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15265         assertFalse(calibrator.isReady());
15266         assertFalse(calibrator.isRunning());
15267         assertNull(calibrator.getEstimatedMa());
15268         assertNull(calibrator.getEstimatedSx());
15269         assertNull(calibrator.getEstimatedSy());
15270         assertNull(calibrator.getEstimatedSz());
15271         assertNull(calibrator.getEstimatedMxy());
15272         assertNull(calibrator.getEstimatedMxz());
15273         assertNull(calibrator.getEstimatedMyx());
15274         assertNull(calibrator.getEstimatedMyz());
15275         assertNull(calibrator.getEstimatedMzx());
15276         assertNull(calibrator.getEstimatedMzy());
15277         assertNull(calibrator.getEstimatedCovariance());
15278         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15279         assertNotNull(calibrator.getGroundTruthGravityNorm());
15280         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15281         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15282         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15283                 ABSOLUTE_ERROR));
15284         final var g = new Acceleration(0.0, AccelerationUnit.G);
15285         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15286         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15287         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15288 
15289         // Force IllegalArgumentException
15290         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15291                 -gravityNorm, measurements, bias));
15292         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15293                 gravityNorm, measurements, new double[1]));
15294     }
15295 
15296     @Test
15297     void testConstructor136() throws WrongSizeException {
15298         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15299 
15300         final var ba = generateBa();
15301         final var bias = ba.getBuffer();
15302         final var biasX = ba.getElementAtIndex(0);
15303         final var biasY = ba.getElementAtIndex(1);
15304         final var biasZ = ba.getElementAtIndex(2);
15305 
15306         final var randomizer = new UniformRandomizer();
15307         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15308         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15309         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15310         final var nedPosition = new NEDPosition(latitude, longitude, height);
15311         final var nedVelocity = new NEDVelocity();
15312         final var ecefPosition = new ECEFPosition();
15313         final var ecefVelocity = new ECEFVelocity();
15314         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15315         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15316                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15317         final var gravityNorm = gravity.getNorm();
15318 
15319         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias,
15320                 this);
15321 
15322         // check default values
15323         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15324         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15325         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15326         final var bx1 = calibrator.getBiasXAsAcceleration();
15327         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15328         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15329         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15330         calibrator.getBiasXAsAcceleration(bx2);
15331         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15332         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15333         final var by1 = calibrator.getBiasYAsAcceleration();
15334         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15335         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15336         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15337         calibrator.getBiasYAsAcceleration(by2);
15338         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15339         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15340         final var bz1 = calibrator.getBiasZAsAcceleration();
15341         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15342         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15343         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15344         calibrator.getBiasZAsAcceleration(bz2);
15345         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15346         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15347         final var biasTriad1 = calibrator.getBiasAsTriad();
15348         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15349         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15350         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15351         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15352         final var biasTriad2 = new AccelerationTriad();
15353         calibrator.getBiasAsTriad(biasTriad2);
15354         assertEquals(biasTriad1, biasTriad2);
15355         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15356         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15357         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15358         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15359         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15360         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15361         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15362         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15363         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15364         final var bias1 = calibrator.getBias();
15365         assertArrayEquals(bias1, bias, 0.0);
15366         final var bias2 = new double[3];
15367         calibrator.getBias(bias2);
15368         assertArrayEquals(bias1, bias2, 0.0);
15369         final var b1 = calibrator.getBiasAsMatrix();
15370         assertEquals(b1, ba);
15371         final var b2 = new Matrix(3, 1);
15372         calibrator.getBiasAsMatrix(b2);
15373         assertEquals(b1, b2);
15374         final var ma1 = calibrator.getInitialMa();
15375         assertEquals(new Matrix(3, 3), ma1);
15376         final var ma2 = new Matrix(3, 3);
15377         calibrator.getInitialMa(ma2);
15378         assertEquals(ma1, ma2);
15379         assertSame(measurements, calibrator.getMeasurements());
15380         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15381                 calibrator.getMeasurementType());
15382         assertFalse(calibrator.isOrderedMeasurementsRequired());
15383         assertFalse(calibrator.isQualityScoresRequired());
15384         assertFalse(calibrator.isCommonAxisUsed());
15385         assertSame(this, calibrator.getListener());
15386         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15387         assertFalse(calibrator.isReady());
15388         assertFalse(calibrator.isRunning());
15389         assertNull(calibrator.getEstimatedMa());
15390         assertNull(calibrator.getEstimatedSx());
15391         assertNull(calibrator.getEstimatedSy());
15392         assertNull(calibrator.getEstimatedSz());
15393         assertNull(calibrator.getEstimatedMxy());
15394         assertNull(calibrator.getEstimatedMxz());
15395         assertNull(calibrator.getEstimatedMyx());
15396         assertNull(calibrator.getEstimatedMyz());
15397         assertNull(calibrator.getEstimatedMzx());
15398         assertNull(calibrator.getEstimatedMzy());
15399         assertNull(calibrator.getEstimatedCovariance());
15400         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15401         assertNotNull(calibrator.getGroundTruthGravityNorm());
15402         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15403         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15404         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15405                 ABSOLUTE_ERROR));
15406         final var g = new Acceleration(0.0, AccelerationUnit.G);
15407         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15408         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15409         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15410 
15411         // Force IllegalArgumentException
15412         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15413                 -gravityNorm, measurements, bias, this));
15414         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15415                 gravityNorm, measurements, new double[1], this));
15416     }
15417 
15418     @Test
15419     void testConstructor137() throws WrongSizeException {
15420         final var ba = generateBa();
15421         final var bias = ba.getBuffer();
15422         final var biasX = ba.getElementAtIndex(0);
15423         final var biasY = ba.getElementAtIndex(1);
15424         final var biasZ = ba.getElementAtIndex(2);
15425 
15426         final var randomizer = new UniformRandomizer();
15427         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15428         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15429         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15430         final var nedPosition = new NEDPosition(latitude, longitude, height);
15431         final var nedVelocity = new NEDVelocity();
15432         final var ecefPosition = new ECEFPosition();
15433         final var ecefVelocity = new ECEFVelocity();
15434         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15435         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15436                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15437         final var gravityNorm = gravity.getNorm();
15438 
15439         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
15440                 bias);
15441 
15442         // check default values
15443         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15444         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15445         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15446         final var bx1 = calibrator.getBiasXAsAcceleration();
15447         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15448         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15449         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15450         calibrator.getBiasXAsAcceleration(bx2);
15451         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15452         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15453         final var by1 = calibrator.getBiasYAsAcceleration();
15454         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15455         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15456         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15457         calibrator.getBiasYAsAcceleration(by2);
15458         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15459         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15460         final var bz1 = calibrator.getBiasZAsAcceleration();
15461         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15462         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15463         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15464         calibrator.getBiasZAsAcceleration(bz2);
15465         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15466         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15467         final var biasTriad1 = calibrator.getBiasAsTriad();
15468         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15469         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15470         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15471         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15472         final var biasTriad2 = new AccelerationTriad();
15473         calibrator.getBiasAsTriad(biasTriad2);
15474         assertEquals(biasTriad1, biasTriad2);
15475         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15476         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15477         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15478         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15479         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15480         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15481         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15482         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15483         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15484         final var bias1 = calibrator.getBias();
15485         assertArrayEquals(bias1, bias, 0.0);
15486         final var bias2 = new double[3];
15487         calibrator.getBias(bias2);
15488         assertArrayEquals(bias1, bias2, 0.0);
15489         final var b1 = calibrator.getBiasAsMatrix();
15490         assertEquals(b1, ba);
15491         final var b2 = new Matrix(3, 1);
15492         calibrator.getBiasAsMatrix(b2);
15493         assertEquals(b1, b2);
15494         final var ma1 = calibrator.getInitialMa();
15495         assertEquals(new Matrix(3, 3), ma1);
15496         final var ma2 = new Matrix(3, 3);
15497         calibrator.getInitialMa(ma2);
15498         assertEquals(ma1, ma2);
15499         assertNull(calibrator.getMeasurements());
15500         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15501                 calibrator.getMeasurementType());
15502         assertFalse(calibrator.isOrderedMeasurementsRequired());
15503         assertFalse(calibrator.isQualityScoresRequired());
15504         assertTrue(calibrator.isCommonAxisUsed());
15505         assertNull(calibrator.getListener());
15506         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15507         assertFalse(calibrator.isReady());
15508         assertFalse(calibrator.isRunning());
15509         assertNull(calibrator.getEstimatedMa());
15510         assertNull(calibrator.getEstimatedSx());
15511         assertNull(calibrator.getEstimatedSy());
15512         assertNull(calibrator.getEstimatedSz());
15513         assertNull(calibrator.getEstimatedMxy());
15514         assertNull(calibrator.getEstimatedMxz());
15515         assertNull(calibrator.getEstimatedMyx());
15516         assertNull(calibrator.getEstimatedMyz());
15517         assertNull(calibrator.getEstimatedMzx());
15518         assertNull(calibrator.getEstimatedMzy());
15519         assertNull(calibrator.getEstimatedCovariance());
15520         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15521         assertNotNull(calibrator.getGroundTruthGravityNorm());
15522         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15523         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15524         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15525                 ABSOLUTE_ERROR));
15526         final var g = new Acceleration(0.0, AccelerationUnit.G);
15527         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15528         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15529         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15530 
15531         // Force IllegalArgumentException
15532         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15533                 -gravityNorm, true, bias));
15534         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15535                 gravityNorm, true, new double[1]));
15536     }
15537 
15538     @Test
15539     void testConstructor138() throws WrongSizeException {
15540         final var ba = generateBa();
15541         final var bias = ba.getBuffer();
15542         final var biasX = ba.getElementAtIndex(0);
15543         final var biasY = ba.getElementAtIndex(1);
15544         final var biasZ = ba.getElementAtIndex(2);
15545 
15546         final var randomizer = new UniformRandomizer();
15547         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15548         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15549         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15550         final var nedPosition = new NEDPosition(latitude, longitude, height);
15551         final var nedVelocity = new NEDVelocity();
15552         final var ecefPosition = new ECEFPosition();
15553         final var ecefVelocity = new ECEFVelocity();
15554         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15555         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15556                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15557         final var gravityNorm = gravity.getNorm();
15558 
15559         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
15560                 bias, this);
15561 
15562         // check default values
15563         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15564         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15565         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15566         final var bx1 = calibrator.getBiasXAsAcceleration();
15567         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15568         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15569         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15570         calibrator.getBiasXAsAcceleration(bx2);
15571         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15572         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15573         final var by1 = calibrator.getBiasYAsAcceleration();
15574         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15575         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15576         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15577         calibrator.getBiasYAsAcceleration(by2);
15578         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15579         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15580         final var bz1 = calibrator.getBiasZAsAcceleration();
15581         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15582         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15583         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15584         calibrator.getBiasZAsAcceleration(bz2);
15585         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15586         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15587         final var biasTriad1 = calibrator.getBiasAsTriad();
15588         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15589         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15590         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15591         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15592         final var biasTriad2 = new AccelerationTriad();
15593         calibrator.getBiasAsTriad(biasTriad2);
15594         assertEquals(biasTriad1, biasTriad2);
15595         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15596         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15597         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15598         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15599         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15600         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15601         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15602         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15603         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15604         final var bias1 = calibrator.getBias();
15605         assertArrayEquals(bias1, bias, 0.0);
15606         final var bias2 = new double[3];
15607         calibrator.getBias(bias2);
15608         assertArrayEquals(bias1, bias2, 0.0);
15609         final var b1 = calibrator.getBiasAsMatrix();
15610         assertEquals(b1, ba);
15611         final var b2 = new Matrix(3, 1);
15612         calibrator.getBiasAsMatrix(b2);
15613         assertEquals(b1, b2);
15614         final var ma1 = calibrator.getInitialMa();
15615         assertEquals(new Matrix(3, 3), ma1);
15616         final var ma2 = new Matrix(3, 3);
15617         calibrator.getInitialMa(ma2);
15618         assertEquals(ma1, ma2);
15619         assertNull(calibrator.getMeasurements());
15620         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15621                 calibrator.getMeasurementType());
15622         assertFalse(calibrator.isOrderedMeasurementsRequired());
15623         assertFalse(calibrator.isQualityScoresRequired());
15624         assertTrue(calibrator.isCommonAxisUsed());
15625         assertSame(this, calibrator.getListener());
15626         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15627         assertFalse(calibrator.isReady());
15628         assertFalse(calibrator.isRunning());
15629         assertNull(calibrator.getEstimatedMa());
15630         assertNull(calibrator.getEstimatedSx());
15631         assertNull(calibrator.getEstimatedSy());
15632         assertNull(calibrator.getEstimatedSz());
15633         assertNull(calibrator.getEstimatedMxy());
15634         assertNull(calibrator.getEstimatedMxz());
15635         assertNull(calibrator.getEstimatedMyx());
15636         assertNull(calibrator.getEstimatedMyz());
15637         assertNull(calibrator.getEstimatedMzx());
15638         assertNull(calibrator.getEstimatedMzy());
15639         assertNull(calibrator.getEstimatedCovariance());
15640         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15641         assertNotNull(calibrator.getGroundTruthGravityNorm());
15642         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15643         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15644         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15645                 ABSOLUTE_ERROR));
15646         final var g = new Acceleration(0.0, AccelerationUnit.G);
15647         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15648         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15649         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15650 
15651         // Force IllegalArgumentException
15652         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15653                 -gravityNorm, true, bias, this));
15654         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15655                 gravityNorm, true, new double[1], this));
15656     }
15657 
15658     @Test
15659     void testConstructor139() throws WrongSizeException {
15660         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15661 
15662         final var ba = generateBa();
15663         final var bias = ba.getBuffer();
15664         final var biasX = ba.getElementAtIndex(0);
15665         final var biasY = ba.getElementAtIndex(1);
15666         final var biasZ = ba.getElementAtIndex(2);
15667 
15668         final var randomizer = new UniformRandomizer();
15669         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15670         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15671         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15672         final var nedPosition = new NEDPosition(latitude, longitude, height);
15673         final var nedVelocity = new NEDVelocity();
15674         final var ecefPosition = new ECEFPosition();
15675         final var ecefVelocity = new ECEFVelocity();
15676         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15677         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15678                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15679         final var gravityNorm = gravity.getNorm();
15680 
15681         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
15682                 true, bias);
15683 
15684         // check default values
15685         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15686         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15687         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15688         final var bx1 = calibrator.getBiasXAsAcceleration();
15689         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15690         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15691         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15692         calibrator.getBiasXAsAcceleration(bx2);
15693         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15694         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15695         final var by1 = calibrator.getBiasYAsAcceleration();
15696         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15697         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15698         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15699         calibrator.getBiasYAsAcceleration(by2);
15700         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15701         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15702         final var bz1 = calibrator.getBiasZAsAcceleration();
15703         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15704         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15705         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15706         calibrator.getBiasZAsAcceleration(bz2);
15707         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15708         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15709         final var biasTriad1 = calibrator.getBiasAsTriad();
15710         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15711         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15712         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15713         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15714         final var biasTriad2 = new AccelerationTriad();
15715         calibrator.getBiasAsTriad(biasTriad2);
15716         assertEquals(biasTriad1, biasTriad2);
15717         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15718         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15719         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15720         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15721         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15722         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15723         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15724         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15725         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15726         final var bias1 = calibrator.getBias();
15727         assertArrayEquals(bias1, bias, 0.0);
15728         final var bias2 = new double[3];
15729         calibrator.getBias(bias2);
15730         assertArrayEquals(bias1, bias2, 0.0);
15731         final var b1 = calibrator.getBiasAsMatrix();
15732         assertEquals(b1, ba);
15733         final var b2 = new Matrix(3, 1);
15734         calibrator.getBiasAsMatrix(b2);
15735         assertEquals(b1, b2);
15736         final var ma1 = calibrator.getInitialMa();
15737         assertEquals(new Matrix(3, 3), ma1);
15738         final var ma2 = new Matrix(3, 3);
15739         calibrator.getInitialMa(ma2);
15740         assertEquals(ma1, ma2);
15741         assertSame(measurements, calibrator.getMeasurements());
15742         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15743                 calibrator.getMeasurementType());
15744         assertFalse(calibrator.isOrderedMeasurementsRequired());
15745         assertFalse(calibrator.isQualityScoresRequired());
15746         assertTrue(calibrator.isCommonAxisUsed());
15747         assertNull(calibrator.getListener());
15748         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15749         assertFalse(calibrator.isReady());
15750         assertFalse(calibrator.isRunning());
15751         assertNull(calibrator.getEstimatedMa());
15752         assertNull(calibrator.getEstimatedSx());
15753         assertNull(calibrator.getEstimatedSy());
15754         assertNull(calibrator.getEstimatedSz());
15755         assertNull(calibrator.getEstimatedMxy());
15756         assertNull(calibrator.getEstimatedMxz());
15757         assertNull(calibrator.getEstimatedMyx());
15758         assertNull(calibrator.getEstimatedMyz());
15759         assertNull(calibrator.getEstimatedMzx());
15760         assertNull(calibrator.getEstimatedMzy());
15761         assertNull(calibrator.getEstimatedCovariance());
15762         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15763         assertNotNull(calibrator.getGroundTruthGravityNorm());
15764         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15765         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15766         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15767                 ABSOLUTE_ERROR));
15768         final var g = new Acceleration(0.0, AccelerationUnit.G);
15769         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15770         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15771         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15772 
15773         // Force IllegalArgumentException
15774         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15775                 -gravityNorm, measurements, true, bias));
15776         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15777                 gravityNorm, measurements, true, new double[1]));
15778     }
15779 
15780     @Test
15781     void testConstructor140() throws WrongSizeException {
15782         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
15783 
15784         final var ba = generateBa();
15785         final var bias = ba.getBuffer();
15786         final var biasX = ba.getElementAtIndex(0);
15787         final var biasY = ba.getElementAtIndex(1);
15788         final var biasZ = ba.getElementAtIndex(2);
15789 
15790         final var randomizer = new UniformRandomizer();
15791         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15792         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15793         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15794         final var nedPosition = new NEDPosition(latitude, longitude, height);
15795         final var nedVelocity = new NEDVelocity();
15796         final var ecefPosition = new ECEFPosition();
15797         final var ecefVelocity = new ECEFVelocity();
15798         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15799         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15800                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15801         final var gravityNorm = gravity.getNorm();
15802 
15803         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
15804                 true, bias, this);
15805 
15806         // check default values
15807         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15808         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15809         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15810         final var bx1 = calibrator.getBiasXAsAcceleration();
15811         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15812         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15813         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15814         calibrator.getBiasXAsAcceleration(bx2);
15815         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15816         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15817         final var by1 = calibrator.getBiasYAsAcceleration();
15818         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15819         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15820         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15821         calibrator.getBiasYAsAcceleration(by2);
15822         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15823         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15824         final var bz1 = calibrator.getBiasZAsAcceleration();
15825         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15826         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15827         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15828         calibrator.getBiasZAsAcceleration(bz2);
15829         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15830         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15831         final var biasTriad1 = calibrator.getBiasAsTriad();
15832         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15833         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15834         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15835         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15836         final var biasTriad2 = new AccelerationTriad();
15837         calibrator.getBiasAsTriad(biasTriad2);
15838         assertEquals(biasTriad1, biasTriad2);
15839         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15840         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15841         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15842         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15843         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15844         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15845         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15846         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15847         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15848         final var bias1 = calibrator.getBias();
15849         assertArrayEquals(bias1, bias, 0.0);
15850         final var bias2 = new double[3];
15851         calibrator.getBias(bias2);
15852         assertArrayEquals(bias1, bias2, 0.0);
15853         final var b1 = calibrator.getBiasAsMatrix();
15854         assertEquals(b1, ba);
15855         final var b2 = new Matrix(3, 1);
15856         calibrator.getBiasAsMatrix(b2);
15857         assertEquals(b1, b2);
15858         final var ma1 = calibrator.getInitialMa();
15859         assertEquals(new Matrix(3, 3), ma1);
15860         final var ma2 = new Matrix(3, 3);
15861         calibrator.getInitialMa(ma2);
15862         assertEquals(ma1, ma2);
15863         assertSame(measurements, calibrator.getMeasurements());
15864         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15865                 calibrator.getMeasurementType());
15866         assertFalse(calibrator.isOrderedMeasurementsRequired());
15867         assertFalse(calibrator.isQualityScoresRequired());
15868         assertTrue(calibrator.isCommonAxisUsed());
15869         assertSame(this, calibrator.getListener());
15870         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
15871         assertFalse(calibrator.isReady());
15872         assertFalse(calibrator.isRunning());
15873         assertNull(calibrator.getEstimatedMa());
15874         assertNull(calibrator.getEstimatedSx());
15875         assertNull(calibrator.getEstimatedSy());
15876         assertNull(calibrator.getEstimatedSz());
15877         assertNull(calibrator.getEstimatedMxy());
15878         assertNull(calibrator.getEstimatedMxz());
15879         assertNull(calibrator.getEstimatedMyx());
15880         assertNull(calibrator.getEstimatedMyz());
15881         assertNull(calibrator.getEstimatedMzx());
15882         assertNull(calibrator.getEstimatedMzy());
15883         assertNull(calibrator.getEstimatedCovariance());
15884         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
15885         assertNotNull(calibrator.getGroundTruthGravityNorm());
15886         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15887         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15888         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
15889                 ABSOLUTE_ERROR));
15890         final var g = new Acceleration(0.0, AccelerationUnit.G);
15891         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15892         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15893         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
15894 
15895         // Force IllegalArgumentException
15896         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15897                 -gravityNorm, measurements, true, bias, this));
15898         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
15899                 gravityNorm, measurements, true, new double[1], this));
15900     }
15901 
15902     @Test
15903     void testConstructor141() throws WrongSizeException {
15904         final var ba = generateBa();
15905         final var bias = ba.getBuffer();
15906         final var biasX = ba.getElementAtIndex(0);
15907         final var biasY = ba.getElementAtIndex(1);
15908         final var biasZ = ba.getElementAtIndex(2);
15909 
15910         final var randomizer = new UniformRandomizer();
15911         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
15912         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15913         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15914         final var nedPosition = new NEDPosition(latitude, longitude, height);
15915         final var nedVelocity = new NEDVelocity();
15916         final var ecefPosition = new ECEFPosition();
15917         final var ecefVelocity = new ECEFVelocity();
15918         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
15919         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15920                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15921         final var gravityNorm = gravity.getNorm();
15922 
15923         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba);
15924 
15925         // check default values
15926         assertEquals(biasX, calibrator.getBiasX(), 0.0);
15927         assertEquals(biasY, calibrator.getBiasY(), 0.0);
15928         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
15929         final var bx1 = calibrator.getBiasXAsAcceleration();
15930         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
15931         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
15932         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15933         calibrator.getBiasXAsAcceleration(bx2);
15934         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
15935         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
15936         final var by1 = calibrator.getBiasYAsAcceleration();
15937         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
15938         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
15939         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15940         calibrator.getBiasYAsAcceleration(by2);
15941         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
15942         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
15943         final var bz1 = calibrator.getBiasZAsAcceleration();
15944         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
15945         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
15946         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
15947         calibrator.getBiasZAsAcceleration(bz2);
15948         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
15949         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
15950         final var biasTriad1 = calibrator.getBiasAsTriad();
15951         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
15952         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
15953         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
15954         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
15955         final var biasTriad2 = new AccelerationTriad();
15956         calibrator.getBiasAsTriad(biasTriad2);
15957         assertEquals(biasTriad1, biasTriad2);
15958         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
15959         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
15960         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
15961         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
15962         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
15963         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
15964         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
15965         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
15966         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
15967         final var bias1 = calibrator.getBias();
15968         assertArrayEquals(bias1, bias, 0.0);
15969         final var bias2 = new double[3];
15970         calibrator.getBias(bias2);
15971         assertArrayEquals(bias1, bias2, 0.0);
15972         final var b1 = calibrator.getBiasAsMatrix();
15973         assertEquals(b1, ba);
15974         final var b2 = new Matrix(3, 1);
15975         calibrator.getBiasAsMatrix(b2);
15976         assertEquals(b1, b2);
15977         final var ma1 = calibrator.getInitialMa();
15978         assertEquals(new Matrix(3, 3), ma1);
15979         final var ma2 = new Matrix(3, 3);
15980         calibrator.getInitialMa(ma2);
15981         assertEquals(ma1, ma2);
15982         assertNull(calibrator.getMeasurements());
15983         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
15984                 calibrator.getMeasurementType());
15985         assertFalse(calibrator.isOrderedMeasurementsRequired());
15986         assertFalse(calibrator.isQualityScoresRequired());
15987         assertFalse(calibrator.isCommonAxisUsed());
15988         assertNull(calibrator.getListener());
15989         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
15990         assertFalse(calibrator.isReady());
15991         assertFalse(calibrator.isRunning());
15992         assertNull(calibrator.getEstimatedMa());
15993         assertNull(calibrator.getEstimatedSx());
15994         assertNull(calibrator.getEstimatedSy());
15995         assertNull(calibrator.getEstimatedSz());
15996         assertNull(calibrator.getEstimatedMxy());
15997         assertNull(calibrator.getEstimatedMxz());
15998         assertNull(calibrator.getEstimatedMyx());
15999         assertNull(calibrator.getEstimatedMyz());
16000         assertNull(calibrator.getEstimatedMzx());
16001         assertNull(calibrator.getEstimatedMzy());
16002         assertNull(calibrator.getEstimatedCovariance());
16003         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16004         assertNotNull(calibrator.getGroundTruthGravityNorm());
16005         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16006         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16007         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16008                 ABSOLUTE_ERROR));
16009         final var g = new Acceleration(0.0, AccelerationUnit.G);
16010         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16011         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16012         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16013 
16014         // Force IllegalArgumentException
16015         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16016                 -gravityNorm, ba));
16017         final var m1 = new Matrix(1, 1);
16018         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16019                 gravityNorm, m1));
16020         final var m2 = new Matrix(1, 3);
16021         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16022                 gravityNorm, m2));
16023     }
16024 
16025     @Test
16026     void testConstructor142() throws WrongSizeException {
16027         final var ba = generateBa();
16028         final var bias = ba.getBuffer();
16029         final var biasX = ba.getElementAtIndex(0);
16030         final var biasY = ba.getElementAtIndex(1);
16031         final var biasZ = ba.getElementAtIndex(2);
16032 
16033         final var randomizer = new UniformRandomizer();
16034         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16035         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16036         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16037         final var nedPosition = new NEDPosition(latitude, longitude, height);
16038         final var nedVelocity = new NEDVelocity();
16039         final var ecefPosition = new ECEFPosition();
16040         final var ecefVelocity = new ECEFVelocity();
16041         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16042         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16043                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16044         final var gravityNorm = gravity.getNorm();
16045 
16046         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, this);
16047 
16048         // check default values
16049         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16050         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16051         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16052         final var bx1 = calibrator.getBiasXAsAcceleration();
16053         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16054         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16055         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16056         calibrator.getBiasXAsAcceleration(bx2);
16057         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16058         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16059         final var by1 = calibrator.getBiasYAsAcceleration();
16060         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16061         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16062         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16063         calibrator.getBiasYAsAcceleration(by2);
16064         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16065         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16066         final var bz1 = calibrator.getBiasZAsAcceleration();
16067         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16068         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16069         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16070         calibrator.getBiasZAsAcceleration(bz2);
16071         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16072         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16073         final var biasTriad1 = calibrator.getBiasAsTriad();
16074         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16075         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16076         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16077         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16078         final var biasTriad2 = new AccelerationTriad();
16079         calibrator.getBiasAsTriad(biasTriad2);
16080         assertEquals(biasTriad1, biasTriad2);
16081         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16082         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16083         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16084         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16085         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16086         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16087         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16088         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16089         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16090         final var bias1 = calibrator.getBias();
16091         assertArrayEquals(bias1, bias, 0.0);
16092         final var bias2 = new double[3];
16093         calibrator.getBias(bias2);
16094         assertArrayEquals(bias1, bias2, 0.0);
16095         final var b1 = calibrator.getBiasAsMatrix();
16096         assertEquals(b1, ba);
16097         final var b2 = new Matrix(3, 1);
16098         calibrator.getBiasAsMatrix(b2);
16099         assertEquals(b1, b2);
16100         final var ma1 = calibrator.getInitialMa();
16101         assertEquals(new Matrix(3, 3), ma1);
16102         final var ma2 = new Matrix(3, 3);
16103         calibrator.getInitialMa(ma2);
16104         assertEquals(ma1, ma2);
16105         assertNull(calibrator.getMeasurements());
16106         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16107                 calibrator.getMeasurementType());
16108         assertFalse(calibrator.isOrderedMeasurementsRequired());
16109         assertFalse(calibrator.isQualityScoresRequired());
16110         assertFalse(calibrator.isCommonAxisUsed());
16111         assertSame(this, calibrator.getListener());
16112         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
16113         assertFalse(calibrator.isReady());
16114         assertFalse(calibrator.isRunning());
16115         assertNull(calibrator.getEstimatedMa());
16116         assertNull(calibrator.getEstimatedSx());
16117         assertNull(calibrator.getEstimatedSy());
16118         assertNull(calibrator.getEstimatedSz());
16119         assertNull(calibrator.getEstimatedMxy());
16120         assertNull(calibrator.getEstimatedMxz());
16121         assertNull(calibrator.getEstimatedMyx());
16122         assertNull(calibrator.getEstimatedMyz());
16123         assertNull(calibrator.getEstimatedMzx());
16124         assertNull(calibrator.getEstimatedMzy());
16125         assertNull(calibrator.getEstimatedCovariance());
16126         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16127         assertNotNull(calibrator.getGroundTruthGravityNorm());
16128         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16129         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16130         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16131                 ABSOLUTE_ERROR));
16132         final var g = new Acceleration(0.0, AccelerationUnit.G);
16133         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16134         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16135         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16136 
16137         // Force IllegalArgumentException
16138         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16139                 -gravityNorm, ba, this));
16140         final var m1 = new Matrix(1, 1);
16141         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16142                 gravityNorm, m1, this));
16143         final var m2 = new Matrix(1, 3);
16144         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16145                 gravityNorm, m2, this));
16146     }
16147 
16148     @Test
16149     void testConstructor143() throws WrongSizeException {
16150         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16151 
16152         final var ba = generateBa();
16153         final var bias = ba.getBuffer();
16154         final var biasX = ba.getElementAtIndex(0);
16155         final var biasY = ba.getElementAtIndex(1);
16156         final var biasZ = ba.getElementAtIndex(2);
16157 
16158         final var randomizer = new UniformRandomizer();
16159         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16160         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16161         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16162         final var nedPosition = new NEDPosition(latitude, longitude, height);
16163         final var nedVelocity = new NEDVelocity();
16164         final var ecefPosition = new ECEFPosition();
16165         final var ecefVelocity = new ECEFVelocity();
16166         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16167         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16168                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16169         final var gravityNorm = gravity.getNorm();
16170 
16171         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba);
16172 
16173         // check default values
16174         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16175         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16176         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16177         final var bx1 = calibrator.getBiasXAsAcceleration();
16178         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16179         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16180         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16181         calibrator.getBiasXAsAcceleration(bx2);
16182         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16183         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16184         final var by1 = calibrator.getBiasYAsAcceleration();
16185         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16186         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16187         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16188         calibrator.getBiasYAsAcceleration(by2);
16189         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16190         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16191         final var bz1 = calibrator.getBiasZAsAcceleration();
16192         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16193         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16194         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16195         calibrator.getBiasZAsAcceleration(bz2);
16196         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16197         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16198         final var biasTriad1 = calibrator.getBiasAsTriad();
16199         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16200         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16201         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16202         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16203         final var biasTriad2 = new AccelerationTriad();
16204         calibrator.getBiasAsTriad(biasTriad2);
16205         assertEquals(biasTriad1, biasTriad2);
16206         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16207         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16208         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16209         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16210         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16211         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16212         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16213         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16214         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16215         final var bias1 = calibrator.getBias();
16216         assertArrayEquals(bias1, bias, 0.0);
16217         final var bias2 = new double[3];
16218         calibrator.getBias(bias2);
16219         assertArrayEquals(bias1, bias2, 0.0);
16220         final var b1 = calibrator.getBiasAsMatrix();
16221         assertEquals(b1, ba);
16222         final var b2 = new Matrix(3, 1);
16223         calibrator.getBiasAsMatrix(b2);
16224         assertEquals(b1, b2);
16225         final var ma1 = calibrator.getInitialMa();
16226         assertEquals(new Matrix(3, 3), ma1);
16227         final var ma2 = new Matrix(3, 3);
16228         calibrator.getInitialMa(ma2);
16229         assertEquals(ma1, ma2);
16230         assertSame(measurements, calibrator.getMeasurements());
16231         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16232                 calibrator.getMeasurementType());
16233         assertFalse(calibrator.isOrderedMeasurementsRequired());
16234         assertFalse(calibrator.isQualityScoresRequired());
16235         assertFalse(calibrator.isCommonAxisUsed());
16236         assertNull(calibrator.getListener());
16237         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
16238         assertFalse(calibrator.isReady());
16239         assertFalse(calibrator.isRunning());
16240         assertNull(calibrator.getEstimatedMa());
16241         assertNull(calibrator.getEstimatedSx());
16242         assertNull(calibrator.getEstimatedSy());
16243         assertNull(calibrator.getEstimatedSz());
16244         assertNull(calibrator.getEstimatedMxy());
16245         assertNull(calibrator.getEstimatedMxz());
16246         assertNull(calibrator.getEstimatedMyx());
16247         assertNull(calibrator.getEstimatedMyz());
16248         assertNull(calibrator.getEstimatedMzx());
16249         assertNull(calibrator.getEstimatedMzy());
16250         assertNull(calibrator.getEstimatedCovariance());
16251         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16252         assertNotNull(calibrator.getGroundTruthGravityNorm());
16253         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16254         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16255         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16256                 ABSOLUTE_ERROR));
16257         final var g = new Acceleration(0.0, AccelerationUnit.G);
16258         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16259         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16260         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16261 
16262         // Force IllegalArgumentException
16263         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16264                 -gravityNorm, measurements, ba));
16265         final var m1 = new Matrix(1, 1);
16266         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16267                 gravityNorm, measurements, m1));
16268         final var m2 = new Matrix(1, 3);
16269         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16270                 gravityNorm, measurements, m2));
16271     }
16272 
16273     @Test
16274     void testConstructor144() throws WrongSizeException {
16275         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16276 
16277         final var ba = generateBa();
16278         final var bias = ba.getBuffer();
16279         final var biasX = ba.getElementAtIndex(0);
16280         final var biasY = ba.getElementAtIndex(1);
16281         final var biasZ = ba.getElementAtIndex(2);
16282 
16283         final var randomizer = new UniformRandomizer();
16284         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16285         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16286         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16287         final var nedPosition = new NEDPosition(latitude, longitude, height);
16288         final var nedVelocity = new NEDVelocity();
16289         final var ecefPosition = new ECEFPosition();
16290         final var ecefVelocity = new ECEFVelocity();
16291         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16292         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16293                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16294         final var gravityNorm = gravity.getNorm();
16295 
16296         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba,
16297                 this);
16298 
16299         // check default values
16300         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16301         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16302         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16303         final var bx1 = calibrator.getBiasXAsAcceleration();
16304         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16305         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16306         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16307         calibrator.getBiasXAsAcceleration(bx2);
16308         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16309         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16310         final var by1 = calibrator.getBiasYAsAcceleration();
16311         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16312         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16313         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16314         calibrator.getBiasYAsAcceleration(by2);
16315         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16316         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16317         final var bz1 = calibrator.getBiasZAsAcceleration();
16318         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16319         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16320         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16321         calibrator.getBiasZAsAcceleration(bz2);
16322         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16323         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16324         final var biasTriad1 = calibrator.getBiasAsTriad();
16325         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16326         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16327         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16328         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16329         final var biasTriad2 = new AccelerationTriad();
16330         calibrator.getBiasAsTriad(biasTriad2);
16331         assertEquals(biasTriad1, biasTriad2);
16332         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16333         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16334         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16335         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16336         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16337         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16338         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16339         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16340         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16341         final var bias1 = calibrator.getBias();
16342         assertArrayEquals(bias1, bias, 0.0);
16343         final var bias2 = new double[3];
16344         calibrator.getBias(bias2);
16345         assertArrayEquals(bias1, bias2, 0.0);
16346         final var b1 = calibrator.getBiasAsMatrix();
16347         assertEquals(b1, ba);
16348         final var b2 = new Matrix(3, 1);
16349         calibrator.getBiasAsMatrix(b2);
16350         assertEquals(b1, b2);
16351         final var ma1 = calibrator.getInitialMa();
16352         assertEquals(new Matrix(3, 3), ma1);
16353         final var ma2 = new Matrix(3, 3);
16354         calibrator.getInitialMa(ma2);
16355         assertEquals(ma1, ma2);
16356         assertSame(measurements, calibrator.getMeasurements());
16357         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16358                 calibrator.getMeasurementType());
16359         assertFalse(calibrator.isOrderedMeasurementsRequired());
16360         assertFalse(calibrator.isQualityScoresRequired());
16361         assertFalse(calibrator.isCommonAxisUsed());
16362         assertSame(this, calibrator.getListener());
16363         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
16364         assertFalse(calibrator.isReady());
16365         assertFalse(calibrator.isRunning());
16366         assertNull(calibrator.getEstimatedMa());
16367         assertNull(calibrator.getEstimatedSx());
16368         assertNull(calibrator.getEstimatedSy());
16369         assertNull(calibrator.getEstimatedSz());
16370         assertNull(calibrator.getEstimatedMxy());
16371         assertNull(calibrator.getEstimatedMxz());
16372         assertNull(calibrator.getEstimatedMyx());
16373         assertNull(calibrator.getEstimatedMyz());
16374         assertNull(calibrator.getEstimatedMzx());
16375         assertNull(calibrator.getEstimatedMzy());
16376         assertNull(calibrator.getEstimatedCovariance());
16377         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16378         assertNotNull(calibrator.getGroundTruthGravityNorm());
16379         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16380         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16381         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16382                 ABSOLUTE_ERROR));
16383         final var g = new Acceleration(0.0, AccelerationUnit.G);
16384         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16385         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16386         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16387 
16388         // Force IllegalArgumentException
16389         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16390                 -gravityNorm, measurements, ba, this));
16391         final var m1 = new Matrix(1, 1);
16392         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16393                 gravityNorm, measurements, m1, this));
16394         final var m2 = new Matrix(1, 3);
16395         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16396                 gravityNorm, measurements, m2, this));
16397     }
16398 
16399     @Test
16400     void testConstructor145() throws WrongSizeException {
16401         final var ba = generateBa();
16402         final var bias = ba.getBuffer();
16403         final var biasX = ba.getElementAtIndex(0);
16404         final var biasY = ba.getElementAtIndex(1);
16405         final var biasZ = ba.getElementAtIndex(2);
16406 
16407         final var randomizer = new UniformRandomizer();
16408         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16409         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16410         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16411         final var nedPosition = new NEDPosition(latitude, longitude, height);
16412         final var nedVelocity = new NEDVelocity();
16413         final var ecefPosition = new ECEFPosition();
16414         final var ecefVelocity = new ECEFVelocity();
16415         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16416         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16417                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16418         final var gravityNorm = gravity.getNorm();
16419 
16420         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba);
16421 
16422         // check default values
16423         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16424         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16425         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16426         final var bx1 = calibrator.getBiasXAsAcceleration();
16427         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16428         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16429         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16430         calibrator.getBiasXAsAcceleration(bx2);
16431         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16432         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16433         final var by1 = calibrator.getBiasYAsAcceleration();
16434         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16435         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16436         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16437         calibrator.getBiasYAsAcceleration(by2);
16438         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16439         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16440         final var bz1 = calibrator.getBiasZAsAcceleration();
16441         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16442         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16443         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16444         calibrator.getBiasZAsAcceleration(bz2);
16445         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16446         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16447         final var biasTriad1 = calibrator.getBiasAsTriad();
16448         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16449         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16450         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16451         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16452         final var biasTriad2 = new AccelerationTriad();
16453         calibrator.getBiasAsTriad(biasTriad2);
16454         assertEquals(biasTriad1, biasTriad2);
16455         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16456         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16457         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16458         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16459         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16460         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16461         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16462         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16463         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16464         final var bias1 = calibrator.getBias();
16465         assertArrayEquals(bias1, bias, 0.0);
16466         final var bias2 = new double[3];
16467         calibrator.getBias(bias2);
16468         assertArrayEquals(bias1, bias2, 0.0);
16469         final var b1 = calibrator.getBiasAsMatrix();
16470         assertEquals(b1, ba);
16471         final var b2 = new Matrix(3, 1);
16472         calibrator.getBiasAsMatrix(b2);
16473         assertEquals(b1, b2);
16474         final var ma1 = calibrator.getInitialMa();
16475         assertEquals(new Matrix(3, 3), ma1);
16476         final var ma2 = new Matrix(3, 3);
16477         calibrator.getInitialMa(ma2);
16478         assertEquals(ma1, ma2);
16479         assertNull(calibrator.getMeasurements());
16480         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16481                 calibrator.getMeasurementType());
16482         assertFalse(calibrator.isOrderedMeasurementsRequired());
16483         assertFalse(calibrator.isQualityScoresRequired());
16484         assertTrue(calibrator.isCommonAxisUsed());
16485         assertNull(calibrator.getListener());
16486         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16487         assertFalse(calibrator.isReady());
16488         assertFalse(calibrator.isRunning());
16489         assertNull(calibrator.getEstimatedMa());
16490         assertNull(calibrator.getEstimatedSx());
16491         assertNull(calibrator.getEstimatedSy());
16492         assertNull(calibrator.getEstimatedSz());
16493         assertNull(calibrator.getEstimatedMxy());
16494         assertNull(calibrator.getEstimatedMxz());
16495         assertNull(calibrator.getEstimatedMyx());
16496         assertNull(calibrator.getEstimatedMyz());
16497         assertNull(calibrator.getEstimatedMzx());
16498         assertNull(calibrator.getEstimatedMzy());
16499         assertNull(calibrator.getEstimatedCovariance());
16500         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16501         assertNotNull(calibrator.getGroundTruthGravityNorm());
16502         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16503         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16504         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16505                 ABSOLUTE_ERROR));
16506         final var g = new Acceleration(0.0, AccelerationUnit.G);
16507         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16508         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16509         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16510 
16511         // Force IllegalArgumentException
16512         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16513                 -gravityNorm, true, ba));
16514         final var m1 = new Matrix(1, 1);
16515         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16516                 gravityNorm, true, m1));
16517         final var m2 = new Matrix(1, 3);
16518         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16519                 gravityNorm, true, m2));
16520     }
16521 
16522     @Test
16523     void testConstructor146() throws WrongSizeException {
16524         final var ba = generateBa();
16525         final var bias = ba.getBuffer();
16526         final var biasX = ba.getElementAtIndex(0);
16527         final var biasY = ba.getElementAtIndex(1);
16528         final var biasZ = ba.getElementAtIndex(2);
16529 
16530         final var randomizer = new UniformRandomizer();
16531         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16532         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16533         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16534         final var nedPosition = new NEDPosition(latitude, longitude, height);
16535         final var nedVelocity = new NEDVelocity();
16536         final var ecefPosition = new ECEFPosition();
16537         final var ecefVelocity = new ECEFVelocity();
16538         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16539         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16540                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16541         final var gravityNorm = gravity.getNorm();
16542 
16543         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba, 
16544                 this);
16545 
16546         // check default values
16547         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16548         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16549         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16550         final var bx1 = calibrator.getBiasXAsAcceleration();
16551         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16552         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16553         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16554         calibrator.getBiasXAsAcceleration(bx2);
16555         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16556         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16557         final var by1 = calibrator.getBiasYAsAcceleration();
16558         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16559         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16560         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16561         calibrator.getBiasYAsAcceleration(by2);
16562         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16563         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16564         final var bz1 = calibrator.getBiasZAsAcceleration();
16565         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16566         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16567         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16568         calibrator.getBiasZAsAcceleration(bz2);
16569         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16570         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16571         final var biasTriad1 = calibrator.getBiasAsTriad();
16572         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16573         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16574         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16575         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16576         final var biasTriad2 = new AccelerationTriad();
16577         calibrator.getBiasAsTriad(biasTriad2);
16578         assertEquals(biasTriad1, biasTriad2);
16579         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16580         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16581         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16582         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16583         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16584         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16585         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16586         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16587         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16588         final var bias1 = calibrator.getBias();
16589         assertArrayEquals(bias1, bias, 0.0);
16590         final var bias2 = new double[3];
16591         calibrator.getBias(bias2);
16592         assertArrayEquals(bias1, bias2, 0.0);
16593         final var b1 = calibrator.getBiasAsMatrix();
16594         assertEquals(b1, ba);
16595         final var b2 = new Matrix(3, 1);
16596         calibrator.getBiasAsMatrix(b2);
16597         assertEquals(b1, b2);
16598         final var ma1 = calibrator.getInitialMa();
16599         assertEquals(new Matrix(3, 3), ma1);
16600         final var ma2 = new Matrix(3, 3);
16601         calibrator.getInitialMa(ma2);
16602         assertEquals(ma1, ma2);
16603         assertNull(calibrator.getMeasurements());
16604         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16605                 calibrator.getMeasurementType());
16606         assertFalse(calibrator.isOrderedMeasurementsRequired());
16607         assertFalse(calibrator.isQualityScoresRequired());
16608         assertTrue(calibrator.isCommonAxisUsed());
16609         assertSame(this, calibrator.getListener());
16610         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16611         assertFalse(calibrator.isReady());
16612         assertFalse(calibrator.isRunning());
16613         assertNull(calibrator.getEstimatedMa());
16614         assertNull(calibrator.getEstimatedSx());
16615         assertNull(calibrator.getEstimatedSy());
16616         assertNull(calibrator.getEstimatedSz());
16617         assertNull(calibrator.getEstimatedMxy());
16618         assertNull(calibrator.getEstimatedMxz());
16619         assertNull(calibrator.getEstimatedMyx());
16620         assertNull(calibrator.getEstimatedMyz());
16621         assertNull(calibrator.getEstimatedMzx());
16622         assertNull(calibrator.getEstimatedMzy());
16623         assertNull(calibrator.getEstimatedCovariance());
16624         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16625         assertNotNull(calibrator.getGroundTruthGravityNorm());
16626         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16627         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16628         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16629                 ABSOLUTE_ERROR));
16630         final var g = new Acceleration(0.0, AccelerationUnit.G);
16631         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16632         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16633         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16634 
16635         // Force IllegalArgumentException
16636         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16637                 -gravityNorm, true, ba, this));
16638         final var m1 = new Matrix(1, 1);
16639         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16640                 gravityNorm, true, m1, this));
16641         final var m2 = new Matrix(1, 3);
16642         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16643                 gravityNorm, true, m2, this));
16644     }
16645 
16646     @Test
16647     void testConstructor147() throws WrongSizeException {
16648         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16649 
16650         final var ba = generateBa();
16651         final var bias = ba.getBuffer();
16652         final var biasX = ba.getElementAtIndex(0);
16653         final var biasY = ba.getElementAtIndex(1);
16654         final var biasZ = ba.getElementAtIndex(2);
16655 
16656         final var randomizer = new UniformRandomizer();
16657         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16658         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16659         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16660         final var nedPosition = new NEDPosition(latitude, longitude, height);
16661         final var nedVelocity = new NEDVelocity();
16662         final var ecefPosition = new ECEFPosition();
16663         final var ecefVelocity = new ECEFVelocity();
16664         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16665         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16666                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16667         final var gravityNorm = gravity.getNorm();
16668 
16669         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
16670                 true, ba);
16671 
16672         // check default values
16673         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16674         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16675         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16676         final var bx1 = calibrator.getBiasXAsAcceleration();
16677         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16678         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16679         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16680         calibrator.getBiasXAsAcceleration(bx2);
16681         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16682         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16683         final var by1 = calibrator.getBiasYAsAcceleration();
16684         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16685         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16686         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16687         calibrator.getBiasYAsAcceleration(by2);
16688         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16689         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16690         final var bz1 = calibrator.getBiasZAsAcceleration();
16691         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16692         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16693         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16694         calibrator.getBiasZAsAcceleration(bz2);
16695         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16696         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16697         final var biasTriad1 = calibrator.getBiasAsTriad();
16698         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16699         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16700         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16701         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16702         final var biasTriad2 = new AccelerationTriad();
16703         calibrator.getBiasAsTriad(biasTriad2);
16704         assertEquals(biasTriad1, biasTriad2);
16705         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16706         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16707         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16708         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16709         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16710         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16711         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16712         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16713         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16714         final var bias1 = calibrator.getBias();
16715         assertArrayEquals(bias1, bias, 0.0);
16716         final var bias2 = new double[3];
16717         calibrator.getBias(bias2);
16718         assertArrayEquals(bias1, bias2, 0.0);
16719         final var b1 = calibrator.getBiasAsMatrix();
16720         assertEquals(b1, ba);
16721         final var b2 = new Matrix(3, 1);
16722         calibrator.getBiasAsMatrix(b2);
16723         assertEquals(b1, b2);
16724         final var ma1 = calibrator.getInitialMa();
16725         assertEquals(new Matrix(3, 3), ma1);
16726         final var ma2 = new Matrix(3, 3);
16727         calibrator.getInitialMa(ma2);
16728         assertEquals(ma1, ma2);
16729         assertSame(measurements, calibrator.getMeasurements());
16730         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16731                 calibrator.getMeasurementType());
16732         assertFalse(calibrator.isOrderedMeasurementsRequired());
16733         assertFalse(calibrator.isQualityScoresRequired());
16734         assertTrue(calibrator.isCommonAxisUsed());
16735         assertNull(calibrator.getListener());
16736         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16737         assertFalse(calibrator.isReady());
16738         assertFalse(calibrator.isRunning());
16739         assertNull(calibrator.getEstimatedMa());
16740         assertNull(calibrator.getEstimatedSx());
16741         assertNull(calibrator.getEstimatedSy());
16742         assertNull(calibrator.getEstimatedSz());
16743         assertNull(calibrator.getEstimatedMxy());
16744         assertNull(calibrator.getEstimatedMxz());
16745         assertNull(calibrator.getEstimatedMyx());
16746         assertNull(calibrator.getEstimatedMyz());
16747         assertNull(calibrator.getEstimatedMzx());
16748         assertNull(calibrator.getEstimatedMzy());
16749         assertNull(calibrator.getEstimatedCovariance());
16750         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16751         assertNotNull(calibrator.getGroundTruthGravityNorm());
16752         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16753         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16754         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16755                 ABSOLUTE_ERROR));
16756         final var g = new Acceleration(0.0, AccelerationUnit.G);
16757         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16758         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16759         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16760 
16761         // Force IllegalArgumentException
16762         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16763                 -gravityNorm, measurements, true, ba));
16764         final var m1 = new Matrix(1, 1);
16765         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16766                 gravityNorm, measurements, true, m1));
16767         final var m2 = new Matrix(1, 3);
16768         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16769                 gravityNorm, measurements, true, m2));
16770     }
16771 
16772     @Test
16773     void testConstructor148() throws WrongSizeException {
16774         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
16775 
16776         final var ba = generateBa();
16777         final var bias = ba.getBuffer();
16778         final var biasX = ba.getElementAtIndex(0);
16779         final var biasY = ba.getElementAtIndex(1);
16780         final var biasZ = ba.getElementAtIndex(2);
16781 
16782         final var randomizer = new UniformRandomizer();
16783         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16784         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16785         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16786         final var nedPosition = new NEDPosition(latitude, longitude, height);
16787         final var nedVelocity = new NEDVelocity();
16788         final var ecefPosition = new ECEFPosition();
16789         final var ecefVelocity = new ECEFVelocity();
16790         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16791         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16792                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16793         final var gravityNorm = gravity.getNorm();
16794 
16795         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
16796                 true, ba, this);
16797 
16798         // check default values
16799         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16800         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16801         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16802         final var bx1 = calibrator.getBiasXAsAcceleration();
16803         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16804         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16805         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16806         calibrator.getBiasXAsAcceleration(bx2);
16807         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16808         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16809         final var by1 = calibrator.getBiasYAsAcceleration();
16810         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16811         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16812         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16813         calibrator.getBiasYAsAcceleration(by2);
16814         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16815         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16816         final var bz1 = calibrator.getBiasZAsAcceleration();
16817         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16818         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16819         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16820         calibrator.getBiasZAsAcceleration(bz2);
16821         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16822         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16823         final var biasTriad1 = calibrator.getBiasAsTriad();
16824         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16825         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16826         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16827         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16828         final var biasTriad2 = new AccelerationTriad();
16829         calibrator.getBiasAsTriad(biasTriad2);
16830         assertEquals(biasTriad1, biasTriad2);
16831         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
16832         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
16833         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
16834         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
16835         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
16836         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
16837         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
16838         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
16839         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
16840         final var bias1 = calibrator.getBias();
16841         assertArrayEquals(bias1, bias, 0.0);
16842         final var bias2 = new double[3];
16843         calibrator.getBias(bias2);
16844         assertArrayEquals(bias1, bias2, 0.0);
16845         final var b1 = calibrator.getBiasAsMatrix();
16846         assertEquals(b1, ba);
16847         final var b2 = new Matrix(3, 1);
16848         calibrator.getBiasAsMatrix(b2);
16849         assertEquals(b1, b2);
16850         final var ma1 = calibrator.getInitialMa();
16851         assertEquals(new Matrix(3, 3), ma1);
16852         final var ma2 = new Matrix(3, 3);
16853         calibrator.getInitialMa(ma2);
16854         assertEquals(ma1, ma2);
16855         assertSame(measurements, calibrator.getMeasurements());
16856         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16857                 calibrator.getMeasurementType());
16858         assertFalse(calibrator.isOrderedMeasurementsRequired());
16859         assertFalse(calibrator.isQualityScoresRequired());
16860         assertTrue(calibrator.isCommonAxisUsed());
16861         assertSame(this, calibrator.getListener());
16862         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
16863         assertFalse(calibrator.isReady());
16864         assertFalse(calibrator.isRunning());
16865         assertNull(calibrator.getEstimatedMa());
16866         assertNull(calibrator.getEstimatedSx());
16867         assertNull(calibrator.getEstimatedSy());
16868         assertNull(calibrator.getEstimatedSz());
16869         assertNull(calibrator.getEstimatedMxy());
16870         assertNull(calibrator.getEstimatedMxz());
16871         assertNull(calibrator.getEstimatedMyx());
16872         assertNull(calibrator.getEstimatedMyz());
16873         assertNull(calibrator.getEstimatedMzx());
16874         assertNull(calibrator.getEstimatedMzy());
16875         assertNull(calibrator.getEstimatedCovariance());
16876         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
16877         assertNotNull(calibrator.getGroundTruthGravityNorm());
16878         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16879         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16880         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
16881                 ABSOLUTE_ERROR));
16882         final var g = new Acceleration(0.0, AccelerationUnit.G);
16883         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16884         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16885         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
16886 
16887         // Force IllegalArgumentException
16888         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16889                 -gravityNorm, measurements, true, ba, this));
16890         final var m1 = new Matrix(1, 1);
16891         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16892                 gravityNorm, measurements, true, m1, this));
16893         final var m2 = new Matrix(1, 3);
16894         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
16895                 gravityNorm, measurements, true, m2, this));
16896     }
16897 
16898     @Test
16899     void testConstructor149() throws WrongSizeException {
16900         final var ba = generateBa();
16901         final var bias = ba.getBuffer();
16902         final var biasX = ba.getElementAtIndex(0);
16903         final var biasY = ba.getElementAtIndex(1);
16904         final var biasZ = ba.getElementAtIndex(2);
16905 
16906         final var ma = generateMaCommonAxis();
16907         final var sx = ma.getElementAt(0, 0);
16908         final var sy = ma.getElementAt(1, 1);
16909         final var sz = ma.getElementAt(2, 2);
16910         final var mxy = ma.getElementAt(0, 1);
16911         final var mxz = ma.getElementAt(0, 2);
16912         final var myx = ma.getElementAt(1, 0);
16913         final var myz = ma.getElementAt(1, 2);
16914         final var mzx = ma.getElementAt(2, 0);
16915         final var mzy = ma.getElementAt(2, 1);
16916 
16917         final var randomizer = new UniformRandomizer();
16918         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
16919         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16920         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16921         final var nedPosition = new NEDPosition(latitude, longitude, height);
16922         final var nedVelocity = new NEDVelocity();
16923         final var ecefPosition = new ECEFPosition();
16924         final var ecefVelocity = new ECEFVelocity();
16925         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
16926         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16927                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16928         final var gravityNorm = gravity.getNorm();
16929 
16930         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma);
16931 
16932         // check default values
16933         assertEquals(biasX, calibrator.getBiasX(), 0.0);
16934         assertEquals(biasY, calibrator.getBiasY(), 0.0);
16935         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
16936         final var bx1 = calibrator.getBiasXAsAcceleration();
16937         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
16938         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
16939         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16940         calibrator.getBiasXAsAcceleration(bx2);
16941         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
16942         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
16943         final var by1 = calibrator.getBiasYAsAcceleration();
16944         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
16945         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
16946         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16947         calibrator.getBiasYAsAcceleration(by2);
16948         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
16949         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
16950         final var bz1 = calibrator.getBiasZAsAcceleration();
16951         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
16952         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
16953         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
16954         calibrator.getBiasZAsAcceleration(bz2);
16955         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
16956         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
16957         final var biasTriad1 = calibrator.getBiasAsTriad();
16958         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
16959         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
16960         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
16961         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
16962         final var biasTriad2 = new AccelerationTriad();
16963         calibrator.getBiasAsTriad(biasTriad2);
16964         assertEquals(biasTriad1, biasTriad2);
16965         assertEquals(sx, calibrator.getInitialSx(), 0.0);
16966         assertEquals(sy, calibrator.getInitialSy(), 0.0);
16967         assertEquals(sz, calibrator.getInitialSz(), 0.0);
16968         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
16969         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
16970         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
16971         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
16972         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
16973         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
16974         final var bias1 = calibrator.getBias();
16975         assertArrayEquals(bias1, bias, 0.0);
16976         final var bias2 = new double[3];
16977         calibrator.getBias(bias2);
16978         assertArrayEquals(bias1, bias2, 0.0);
16979         final var b1 = calibrator.getBiasAsMatrix();
16980         assertEquals(b1, ba);
16981         final var b2 = new Matrix(3, 1);
16982         calibrator.getBiasAsMatrix(b2);
16983         assertEquals(b1, b2);
16984         final var ma1 = new Matrix(3, 3);
16985         ma1.setSubmatrix(0, 0, 2, 2,
16986                 new double[]{sx, myx, mzx,
16987                         mxy, sy, mzy,
16988                         mxz, myz, sz});
16989         assertEquals(ma1, calibrator.getInitialMa());
16990         final var ma2 = new Matrix(3, 3);
16991         calibrator.getInitialMa(ma2);
16992         assertEquals(ma1, ma2);
16993         assertNull(calibrator.getMeasurements());
16994         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
16995                 calibrator.getMeasurementType());
16996         assertFalse(calibrator.isOrderedMeasurementsRequired());
16997         assertFalse(calibrator.isQualityScoresRequired());
16998         assertFalse(calibrator.isCommonAxisUsed());
16999         assertNull(calibrator.getListener());
17000         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17001         assertFalse(calibrator.isReady());
17002         assertFalse(calibrator.isRunning());
17003         assertNull(calibrator.getEstimatedMa());
17004         assertNull(calibrator.getEstimatedSx());
17005         assertNull(calibrator.getEstimatedSy());
17006         assertNull(calibrator.getEstimatedSz());
17007         assertNull(calibrator.getEstimatedMxy());
17008         assertNull(calibrator.getEstimatedMxz());
17009         assertNull(calibrator.getEstimatedMyx());
17010         assertNull(calibrator.getEstimatedMyz());
17011         assertNull(calibrator.getEstimatedMzx());
17012         assertNull(calibrator.getEstimatedMzy());
17013         assertNull(calibrator.getEstimatedCovariance());
17014         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17015         assertNotNull(calibrator.getGroundTruthGravityNorm());
17016         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17017         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17018         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17019                 ABSOLUTE_ERROR));
17020         final var g = new Acceleration(0.0, AccelerationUnit.G);
17021         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17022         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17023         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17024 
17025         // Force IllegalArgumentException
17026         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17027                 -gravityNorm, ba, ma));
17028         final var m1 = new Matrix(1, 1);
17029         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17030                 gravityNorm, m1, ma));
17031         final var m2 = new Matrix(1, 3);
17032         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17033                 gravityNorm, m2, ma));
17034         final var m3 = new Matrix(1, 3);
17035         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17036                 gravityNorm, ba, m3));
17037         final var m4 = new Matrix(3, 1);
17038         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17039                 gravityNorm, ba, m4));
17040     }
17041 
17042     @Test
17043     void testConstructor150() throws WrongSizeException {
17044         final var ba = generateBa();
17045         final var bias = ba.getBuffer();
17046         final var biasX = ba.getElementAtIndex(0);
17047         final var biasY = ba.getElementAtIndex(1);
17048         final var biasZ = ba.getElementAtIndex(2);
17049 
17050         final var ma = generateMaCommonAxis();
17051         final var sx = ma.getElementAt(0, 0);
17052         final var sy = ma.getElementAt(1, 1);
17053         final var sz = ma.getElementAt(2, 2);
17054         final var mxy = ma.getElementAt(0, 1);
17055         final var mxz = ma.getElementAt(0, 2);
17056         final var myx = ma.getElementAt(1, 0);
17057         final var myz = ma.getElementAt(1, 2);
17058         final var mzx = ma.getElementAt(2, 0);
17059         final var mzy = ma.getElementAt(2, 1);
17060 
17061         final var randomizer = new UniformRandomizer();
17062         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17063         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17064         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17065         final var nedPosition = new NEDPosition(latitude, longitude, height);
17066         final var nedVelocity = new NEDVelocity();
17067         final var ecefPosition = new ECEFPosition();
17068         final var ecefVelocity = new ECEFVelocity();
17069         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17070         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17071                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17072         final var gravityNorm = gravity.getNorm();
17073 
17074         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma, this);
17075 
17076         // check default values
17077         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17078         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17079         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17080         final var bx1 = calibrator.getBiasXAsAcceleration();
17081         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17082         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17083         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17084         calibrator.getBiasXAsAcceleration(bx2);
17085         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17086         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17087         final var by1 = calibrator.getBiasYAsAcceleration();
17088         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17089         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17090         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17091         calibrator.getBiasYAsAcceleration(by2);
17092         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17093         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17094         final var bz1 = calibrator.getBiasZAsAcceleration();
17095         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17096         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17097         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17098         calibrator.getBiasZAsAcceleration(bz2);
17099         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17100         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17101         final var biasTriad1 = calibrator.getBiasAsTriad();
17102         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17103         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17104         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17105         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17106         final var biasTriad2 = new AccelerationTriad();
17107         calibrator.getBiasAsTriad(biasTriad2);
17108         assertEquals(biasTriad1, biasTriad2);
17109         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17110         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17111         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17112         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17113         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17114         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17115         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17116         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17117         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17118         final var bias1 = calibrator.getBias();
17119         assertArrayEquals(bias1, bias, 0.0);
17120         final var bias2 = new double[3];
17121         calibrator.getBias(bias2);
17122         assertArrayEquals(bias1, bias2, 0.0);
17123         final var b1 = calibrator.getBiasAsMatrix();
17124         assertEquals(b1, ba);
17125         final var b2 = new Matrix(3, 1);
17126         calibrator.getBiasAsMatrix(b2);
17127         assertEquals(b1, b2);
17128         final var ma1 = new Matrix(3, 3);
17129         ma1.setSubmatrix(0, 0, 2, 2,
17130                 new double[]{sx, myx, mzx,
17131                         mxy, sy, mzy,
17132                         mxz, myz, sz});
17133         assertEquals(ma1, calibrator.getInitialMa());
17134         final var ma2 = new Matrix(3, 3);
17135         calibrator.getInitialMa(ma2);
17136         assertEquals(ma1, ma2);
17137         assertNull(calibrator.getMeasurements());
17138         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17139                 calibrator.getMeasurementType());
17140         assertFalse(calibrator.isOrderedMeasurementsRequired());
17141         assertFalse(calibrator.isQualityScoresRequired());
17142         assertFalse(calibrator.isCommonAxisUsed());
17143         assertSame(this, calibrator.getListener());
17144         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17145         assertFalse(calibrator.isReady());
17146         assertFalse(calibrator.isRunning());
17147         assertNull(calibrator.getEstimatedMa());
17148         assertNull(calibrator.getEstimatedSx());
17149         assertNull(calibrator.getEstimatedSy());
17150         assertNull(calibrator.getEstimatedSz());
17151         assertNull(calibrator.getEstimatedMxy());
17152         assertNull(calibrator.getEstimatedMxz());
17153         assertNull(calibrator.getEstimatedMyx());
17154         assertNull(calibrator.getEstimatedMyz());
17155         assertNull(calibrator.getEstimatedMzx());
17156         assertNull(calibrator.getEstimatedMzy());
17157         assertNull(calibrator.getEstimatedCovariance());
17158         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17159         assertNotNull(calibrator.getGroundTruthGravityNorm());
17160         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17161         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17162         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17163                 ABSOLUTE_ERROR));
17164         final var g = new Acceleration(0.0, AccelerationUnit.G);
17165         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17166         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17167         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17168 
17169         // Force IllegalArgumentException
17170         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17171                 -gravityNorm, ba, ma, this));
17172         final var m1 = new Matrix(1, 1);
17173         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17174                 gravityNorm, m1, ma, this));
17175         final var m2 = new Matrix(1, 3);
17176         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17177                 gravityNorm, m2, ma, this));
17178         final var m3 = new Matrix(1, 3);
17179         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17180                 gravityNorm, ba, m3, this));
17181         final var m4 = new Matrix(3, 1);
17182         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17183                 gravityNorm, ba, m4, this));
17184     }
17185 
17186     @Test
17187     void testConstructor151() throws WrongSizeException {
17188         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17189 
17190         final var ba = generateBa();
17191         final var bias = ba.getBuffer();
17192         final var biasX = ba.getElementAtIndex(0);
17193         final var biasY = ba.getElementAtIndex(1);
17194         final var biasZ = ba.getElementAtIndex(2);
17195 
17196         final var ma = generateMaCommonAxis();
17197         final var sx = ma.getElementAt(0, 0);
17198         final var sy = ma.getElementAt(1, 1);
17199         final var sz = ma.getElementAt(2, 2);
17200         final var mxy = ma.getElementAt(0, 1);
17201         final var mxz = ma.getElementAt(0, 2);
17202         final var myx = ma.getElementAt(1, 0);
17203         final var myz = ma.getElementAt(1, 2);
17204         final var mzx = ma.getElementAt(2, 0);
17205         final var mzy = ma.getElementAt(2, 1);
17206 
17207         final var randomizer = new UniformRandomizer();
17208         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17209         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17210         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17211         final var nedPosition = new NEDPosition(latitude, longitude, height);
17212         final var nedVelocity = new NEDVelocity();
17213         final var ecefPosition = new ECEFPosition();
17214         final var ecefVelocity = new ECEFVelocity();
17215         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17216         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17217                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17218         final var gravityNorm = gravity.getNorm();
17219 
17220         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma);
17221 
17222         // check default values
17223         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17224         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17225         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17226         final var bx1 = calibrator.getBiasXAsAcceleration();
17227         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17228         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17229         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17230         calibrator.getBiasXAsAcceleration(bx2);
17231         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17232         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17233         final var by1 = calibrator.getBiasYAsAcceleration();
17234         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17235         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17236         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17237         calibrator.getBiasYAsAcceleration(by2);
17238         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17239         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17240         final var bz1 = calibrator.getBiasZAsAcceleration();
17241         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17242         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17243         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17244         calibrator.getBiasZAsAcceleration(bz2);
17245         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17246         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17247         final var biasTriad1 = calibrator.getBiasAsTriad();
17248         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17249         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17250         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17251         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17252         final var biasTriad2 = new AccelerationTriad();
17253         calibrator.getBiasAsTriad(biasTriad2);
17254         assertEquals(biasTriad1, biasTriad2);
17255         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17256         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17257         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17258         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17259         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17260         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17261         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17262         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17263         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17264         final var bias1 = calibrator.getBias();
17265         assertArrayEquals(bias1, bias, 0.0);
17266         final var bias2 = new double[3];
17267         calibrator.getBias(bias2);
17268         assertArrayEquals(bias1, bias2, 0.0);
17269         final var b1 = calibrator.getBiasAsMatrix();
17270         assertEquals(b1, ba);
17271         final var b2 = new Matrix(3, 1);
17272         calibrator.getBiasAsMatrix(b2);
17273         assertEquals(b1, b2);
17274         final var ma1 = new Matrix(3, 3);
17275         ma1.setSubmatrix(0, 0, 2, 2,
17276                 new double[]{sx, myx, mzx,
17277                         mxy, sy, mzy,
17278                         mxz, myz, sz});
17279         assertEquals(ma1, calibrator.getInitialMa());
17280         final var ma2 = new Matrix(3, 3);
17281         calibrator.getInitialMa(ma2);
17282         assertEquals(ma1, ma2);
17283         assertSame(measurements, calibrator.getMeasurements());
17284         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17285                 calibrator.getMeasurementType());
17286         assertFalse(calibrator.isOrderedMeasurementsRequired());
17287         assertFalse(calibrator.isQualityScoresRequired());
17288         assertFalse(calibrator.isCommonAxisUsed());
17289         assertNull(calibrator.getListener());
17290         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17291         assertFalse(calibrator.isReady());
17292         assertFalse(calibrator.isRunning());
17293         assertNull(calibrator.getEstimatedMa());
17294         assertNull(calibrator.getEstimatedSx());
17295         assertNull(calibrator.getEstimatedSy());
17296         assertNull(calibrator.getEstimatedSz());
17297         assertNull(calibrator.getEstimatedMxy());
17298         assertNull(calibrator.getEstimatedMxz());
17299         assertNull(calibrator.getEstimatedMyx());
17300         assertNull(calibrator.getEstimatedMyz());
17301         assertNull(calibrator.getEstimatedMzx());
17302         assertNull(calibrator.getEstimatedMzy());
17303         assertNull(calibrator.getEstimatedCovariance());
17304         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17305         assertNotNull(calibrator.getGroundTruthGravityNorm());
17306         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17307         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17308         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17309                 ABSOLUTE_ERROR));
17310         final var g = new Acceleration(0.0, AccelerationUnit.G);
17311         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17312         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17313         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17314 
17315         // Force IllegalArgumentException
17316         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17317                 -gravityNorm, measurements, ba, ma));
17318         final var m1 = new Matrix(1, 1);
17319         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17320                 gravityNorm, measurements, m1, ma));
17321         final var m2 = new Matrix(1, 3);
17322         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17323                 gravityNorm, measurements, m2, ma));
17324         final var m3 = new Matrix(1, 3);
17325         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17326                 gravityNorm, measurements, ba, m3));
17327         final var m4 = new Matrix(3, 1);
17328         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17329                 gravityNorm, measurements, ba, m4));
17330     }
17331 
17332     @Test
17333     void testConstructor152() throws WrongSizeException {
17334         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17335 
17336         final var ba = generateBa();
17337         final var bias = ba.getBuffer();
17338         final var biasX = ba.getElementAtIndex(0);
17339         final var biasY = ba.getElementAtIndex(1);
17340         final var biasZ = ba.getElementAtIndex(2);
17341 
17342         final var ma = generateMaCommonAxis();
17343         final var sx = ma.getElementAt(0, 0);
17344         final var sy = ma.getElementAt(1, 1);
17345         final var sz = ma.getElementAt(2, 2);
17346         final var mxy = ma.getElementAt(0, 1);
17347         final var mxz = ma.getElementAt(0, 2);
17348         final var myx = ma.getElementAt(1, 0);
17349         final var myz = ma.getElementAt(1, 2);
17350         final var mzx = ma.getElementAt(2, 0);
17351         final var mzy = ma.getElementAt(2, 1);
17352 
17353         final var randomizer = new UniformRandomizer();
17354         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17355         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17356         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17357         final var nedPosition = new NEDPosition(latitude, longitude, height);
17358         final var nedVelocity = new NEDVelocity();
17359         final var ecefPosition = new ECEFPosition();
17360         final var ecefVelocity = new ECEFVelocity();
17361         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17362         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17363                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17364         final var gravityNorm = gravity.getNorm();
17365 
17366         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma, 
17367                 this);
17368 
17369         // check default values
17370         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17371         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17372         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17373         final var bx1 = calibrator.getBiasXAsAcceleration();
17374         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17375         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17376         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17377         calibrator.getBiasXAsAcceleration(bx2);
17378         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17379         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17380         final var by1 = calibrator.getBiasYAsAcceleration();
17381         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17382         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17383         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17384         calibrator.getBiasYAsAcceleration(by2);
17385         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17386         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17387         final var bz1 = calibrator.getBiasZAsAcceleration();
17388         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17389         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17390         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17391         calibrator.getBiasZAsAcceleration(bz2);
17392         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17393         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17394         final var biasTriad1 = calibrator.getBiasAsTriad();
17395         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17396         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17397         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17398         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17399         final var biasTriad2 = new AccelerationTriad();
17400         calibrator.getBiasAsTriad(biasTriad2);
17401         assertEquals(biasTriad1, biasTriad2);
17402         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17403         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17404         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17405         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17406         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17407         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17408         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17409         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17410         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17411         final var bias1 = calibrator.getBias();
17412         assertArrayEquals(bias1, bias, 0.0);
17413         final var bias2 = new double[3];
17414         calibrator.getBias(bias2);
17415         assertArrayEquals(bias1, bias2, 0.0);
17416         final var b1 = calibrator.getBiasAsMatrix();
17417         assertEquals(b1, ba);
17418         final var b2 = new Matrix(3, 1);
17419         calibrator.getBiasAsMatrix(b2);
17420         assertEquals(b1, b2);
17421         final var ma1 = new Matrix(3, 3);
17422         ma1.setSubmatrix(0, 0, 2, 2,
17423                 new double[]{sx, myx, mzx,
17424                         mxy, sy, mzy,
17425                         mxz, myz, sz});
17426         assertEquals(ma1, calibrator.getInitialMa());
17427         final var ma2 = new Matrix(3, 3);
17428         calibrator.getInitialMa(ma2);
17429         assertEquals(ma1, ma2);
17430         assertSame(measurements, calibrator.getMeasurements());
17431         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17432                 calibrator.getMeasurementType());
17433         assertFalse(calibrator.isOrderedMeasurementsRequired());
17434         assertFalse(calibrator.isQualityScoresRequired());
17435         assertFalse(calibrator.isCommonAxisUsed());
17436         assertSame(this, calibrator.getListener());
17437         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
17438         assertFalse(calibrator.isReady());
17439         assertFalse(calibrator.isRunning());
17440         assertNull(calibrator.getEstimatedMa());
17441         assertNull(calibrator.getEstimatedSx());
17442         assertNull(calibrator.getEstimatedSy());
17443         assertNull(calibrator.getEstimatedSz());
17444         assertNull(calibrator.getEstimatedMxy());
17445         assertNull(calibrator.getEstimatedMxz());
17446         assertNull(calibrator.getEstimatedMyx());
17447         assertNull(calibrator.getEstimatedMyz());
17448         assertNull(calibrator.getEstimatedMzx());
17449         assertNull(calibrator.getEstimatedMzy());
17450         assertNull(calibrator.getEstimatedCovariance());
17451         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17452         assertNotNull(calibrator.getGroundTruthGravityNorm());
17453         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17454         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17455         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17456                 ABSOLUTE_ERROR));
17457         final var g = new Acceleration(0.0, AccelerationUnit.G);
17458         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17459         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17460         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17461 
17462         // Force IllegalArgumentException
17463         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17464                 -gravityNorm, measurements, ba, ma, this));
17465         final var m1 = new Matrix(1, 1);
17466         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17467                 gravityNorm, measurements, m1, ma, this));
17468         final var m2 = new Matrix(1, 3);
17469         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17470                 gravityNorm, measurements, m2, ma, this));
17471         final var m3 = new Matrix(1, 3);
17472         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17473                 gravityNorm, measurements, ba, m3, this));
17474         final var m4 = new Matrix(3, 1);
17475         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17476                 gravityNorm, measurements, ba, m4, this));
17477     }
17478 
17479     @Test
17480     void testConstructor153() throws WrongSizeException {
17481         final var ba = generateBa();
17482         final var bias = ba.getBuffer();
17483         final var biasX = ba.getElementAtIndex(0);
17484         final var biasY = ba.getElementAtIndex(1);
17485         final var biasZ = ba.getElementAtIndex(2);
17486 
17487         final var ma = generateMaCommonAxis();
17488         final var sx = ma.getElementAt(0, 0);
17489         final var sy = ma.getElementAt(1, 1);
17490         final var sz = ma.getElementAt(2, 2);
17491         final var mxy = ma.getElementAt(0, 1);
17492         final var mxz = ma.getElementAt(0, 2);
17493         final var myx = ma.getElementAt(1, 0);
17494         final var myz = ma.getElementAt(1, 2);
17495         final var mzx = ma.getElementAt(2, 0);
17496         final var mzy = ma.getElementAt(2, 1);
17497 
17498         final var randomizer = new UniformRandomizer();
17499         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17500         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17501         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17502         final var nedPosition = new NEDPosition(latitude, longitude, height);
17503         final var nedVelocity = new NEDVelocity();
17504         final var ecefPosition = new ECEFPosition();
17505         final var ecefVelocity = new ECEFVelocity();
17506         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17507         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17508                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17509         final var gravityNorm = gravity.getNorm();
17510 
17511         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba, ma);
17512 
17513         // check default values
17514         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17515         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17516         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17517         final var bx1 = calibrator.getBiasXAsAcceleration();
17518         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17519         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17520         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17521         calibrator.getBiasXAsAcceleration(bx2);
17522         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17523         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17524         final var by1 = calibrator.getBiasYAsAcceleration();
17525         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17526         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17527         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17528         calibrator.getBiasYAsAcceleration(by2);
17529         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17530         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17531         final var bz1 = calibrator.getBiasZAsAcceleration();
17532         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17533         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17534         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17535         calibrator.getBiasZAsAcceleration(bz2);
17536         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17537         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17538         final var biasTriad1 = calibrator.getBiasAsTriad();
17539         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17540         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17541         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17542         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17543         final var biasTriad2 = new AccelerationTriad();
17544         calibrator.getBiasAsTriad(biasTriad2);
17545         assertEquals(biasTriad1, biasTriad2);
17546         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17547         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17548         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17549         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17550         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17551         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17552         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17553         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17554         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17555         final var bias1 = calibrator.getBias();
17556         assertArrayEquals(bias1, bias, 0.0);
17557         final var bias2 = new double[3];
17558         calibrator.getBias(bias2);
17559         assertArrayEquals(bias1, bias2, 0.0);
17560         final var b1 = calibrator.getBiasAsMatrix();
17561         assertEquals(b1, ba);
17562         final var b2 = new Matrix(3, 1);
17563         calibrator.getBiasAsMatrix(b2);
17564         assertEquals(b1, b2);
17565         final var ma1 = new Matrix(3, 3);
17566         ma1.setSubmatrix(0, 0, 2, 2,
17567                 new double[]{sx, myx, mzx,
17568                         mxy, sy, mzy,
17569                         mxz, myz, sz});
17570         assertEquals(ma1, calibrator.getInitialMa());
17571         final var ma2 = new Matrix(3, 3);
17572         calibrator.getInitialMa(ma2);
17573         assertEquals(ma1, ma2);
17574         assertNull(calibrator.getMeasurements());
17575         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17576                 calibrator.getMeasurementType());
17577         assertFalse(calibrator.isOrderedMeasurementsRequired());
17578         assertFalse(calibrator.isQualityScoresRequired());
17579         assertTrue(calibrator.isCommonAxisUsed());
17580         assertNull(calibrator.getListener());
17581         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
17582         assertFalse(calibrator.isReady());
17583         assertFalse(calibrator.isRunning());
17584         assertNull(calibrator.getEstimatedMa());
17585         assertNull(calibrator.getEstimatedSx());
17586         assertNull(calibrator.getEstimatedSy());
17587         assertNull(calibrator.getEstimatedSz());
17588         assertNull(calibrator.getEstimatedMxy());
17589         assertNull(calibrator.getEstimatedMxz());
17590         assertNull(calibrator.getEstimatedMyx());
17591         assertNull(calibrator.getEstimatedMyz());
17592         assertNull(calibrator.getEstimatedMzx());
17593         assertNull(calibrator.getEstimatedMzy());
17594         assertNull(calibrator.getEstimatedCovariance());
17595         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17596         assertNotNull(calibrator.getGroundTruthGravityNorm());
17597         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17598         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17599         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17600                 ABSOLUTE_ERROR));
17601         final var g = new Acceleration(0.0, AccelerationUnit.G);
17602         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17603         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17604         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17605 
17606         // Force IllegalArgumentException
17607         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17608                 -gravityNorm, true, ba, ma));
17609         final var m1 = new Matrix(1, 1);
17610         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17611                 gravityNorm, true, m1, ma));
17612         final var m2 = new Matrix(1, 3);
17613         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17614                 gravityNorm, true, m2, ma));
17615         final var m3 = new Matrix(1, 3);
17616         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17617                 gravityNorm, true, ba, m3));
17618         final var m4 = new Matrix(3, 1);
17619         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17620                 gravityNorm, true, ba, m4));
17621     }
17622 
17623     @Test
17624     void testConstructor154() throws WrongSizeException {
17625         final var ba = generateBa();
17626         final var bias = ba.getBuffer();
17627         final var biasX = ba.getElementAtIndex(0);
17628         final var biasY = ba.getElementAtIndex(1);
17629         final var biasZ = ba.getElementAtIndex(2);
17630 
17631         final var ma = generateMaCommonAxis();
17632         final var sx = ma.getElementAt(0, 0);
17633         final var sy = ma.getElementAt(1, 1);
17634         final var sz = ma.getElementAt(2, 2);
17635         final var mxy = ma.getElementAt(0, 1);
17636         final var mxz = ma.getElementAt(0, 2);
17637         final var myx = ma.getElementAt(1, 0);
17638         final var myz = ma.getElementAt(1, 2);
17639         final var mzx = ma.getElementAt(2, 0);
17640         final var mzy = ma.getElementAt(2, 1);
17641 
17642         final var randomizer = new UniformRandomizer();
17643         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17644         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17645         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17646         final var nedPosition = new NEDPosition(latitude, longitude, height);
17647         final var nedVelocity = new NEDVelocity();
17648         final var ecefPosition = new ECEFPosition();
17649         final var ecefVelocity = new ECEFVelocity();
17650         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17651         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17652                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17653         final var gravityNorm = gravity.getNorm();
17654 
17655         KnownBiasAndGravityNormAccelerometerCalibrator calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17656                 gravityNorm, true, ba, ma, this);
17657 
17658         // check default values
17659         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17660         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17661         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17662         final var bx1 = calibrator.getBiasXAsAcceleration();
17663         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17664         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17665         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17666         calibrator.getBiasXAsAcceleration(bx2);
17667         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17668         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17669         final var by1 = calibrator.getBiasYAsAcceleration();
17670         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17671         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17672         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17673         calibrator.getBiasYAsAcceleration(by2);
17674         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17675         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17676         final var bz1 = calibrator.getBiasZAsAcceleration();
17677         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17678         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17679         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17680         calibrator.getBiasZAsAcceleration(bz2);
17681         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17682         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17683         final var biasTriad1 = calibrator.getBiasAsTriad();
17684         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17685         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17686         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17687         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17688         final var biasTriad2 = new AccelerationTriad();
17689         calibrator.getBiasAsTriad(biasTriad2);
17690         assertEquals(biasTriad1, biasTriad2);
17691         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17692         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17693         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17694         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17695         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17696         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17697         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17698         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17699         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17700         final var bias1 = calibrator.getBias();
17701         assertArrayEquals(bias1, bias, 0.0);
17702         final var bias2 = new double[3];
17703         calibrator.getBias(bias2);
17704         assertArrayEquals(bias1, bias2, 0.0);
17705         final var b1 = calibrator.getBiasAsMatrix();
17706         assertEquals(b1, ba);
17707         final var b2 = new Matrix(3, 1);
17708         calibrator.getBiasAsMatrix(b2);
17709         assertEquals(b1, b2);
17710         final var ma1 = new Matrix(3, 3);
17711         ma1.setSubmatrix(0, 0, 2, 2,
17712                 new double[]{sx, myx, mzx,
17713                         mxy, sy, mzy,
17714                         mxz, myz, sz});
17715         assertEquals(ma1, calibrator.getInitialMa());
17716         final var ma2 = new Matrix(3, 3);
17717         calibrator.getInitialMa(ma2);
17718         assertEquals(ma1, ma2);
17719         assertNull(calibrator.getMeasurements());
17720         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17721                 calibrator.getMeasurementType());
17722         assertFalse(calibrator.isOrderedMeasurementsRequired());
17723         assertFalse(calibrator.isQualityScoresRequired());
17724         assertTrue(calibrator.isCommonAxisUsed());
17725         assertSame(this, calibrator.getListener());
17726         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
17727         assertFalse(calibrator.isReady());
17728         assertFalse(calibrator.isRunning());
17729         assertNull(calibrator.getEstimatedMa());
17730         assertNull(calibrator.getEstimatedSx());
17731         assertNull(calibrator.getEstimatedSy());
17732         assertNull(calibrator.getEstimatedSz());
17733         assertNull(calibrator.getEstimatedMxy());
17734         assertNull(calibrator.getEstimatedMxz());
17735         assertNull(calibrator.getEstimatedMyx());
17736         assertNull(calibrator.getEstimatedMyz());
17737         assertNull(calibrator.getEstimatedMzx());
17738         assertNull(calibrator.getEstimatedMzy());
17739         assertNull(calibrator.getEstimatedCovariance());
17740         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17741         assertNotNull(calibrator.getGroundTruthGravityNorm());
17742         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17743         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17744         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17745                 ABSOLUTE_ERROR));
17746         final var g = new Acceleration(0.0, AccelerationUnit.G);
17747         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17748         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17749         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17750 
17751         // Force IllegalArgumentException
17752         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17753                 -gravityNorm, true, ba, ma, this));
17754         final var m1 = new Matrix(1, 1);
17755         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17756                 gravityNorm, true, m1, ma, this));
17757         final var m2 = new Matrix(1, 3);
17758         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17759                 gravityNorm, true, m2, ma, this));
17760         final var m3 = new Matrix(1, 3);
17761         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17762                 gravityNorm, true, ba, m3, this));
17763         final var m4 = new Matrix(3, 1);
17764         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17765                 gravityNorm, true, ba, m4, this));
17766     }
17767 
17768     @Test
17769     void testConstructor155() throws WrongSizeException {
17770         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17771 
17772         final var ba = generateBa();
17773         final var bias = ba.getBuffer();
17774         final var biasX = ba.getElementAtIndex(0);
17775         final var biasY = ba.getElementAtIndex(1);
17776         final var biasZ = ba.getElementAtIndex(2);
17777 
17778         final var ma = generateMaCommonAxis();
17779         final var sx = ma.getElementAt(0, 0);
17780         final var sy = ma.getElementAt(1, 1);
17781         final var sz = ma.getElementAt(2, 2);
17782         final var mxy = ma.getElementAt(0, 1);
17783         final var mxz = ma.getElementAt(0, 2);
17784         final var myx = ma.getElementAt(1, 0);
17785         final var myz = ma.getElementAt(1, 2);
17786         final var mzx = ma.getElementAt(2, 0);
17787         final var mzy = ma.getElementAt(2, 1);
17788 
17789         final var randomizer = new UniformRandomizer();
17790         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17791         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17792         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17793         final var nedPosition = new NEDPosition(latitude, longitude, height);
17794         final var nedVelocity = new NEDVelocity();
17795         final var ecefPosition = new ECEFPosition();
17796         final var ecefVelocity = new ECEFVelocity();
17797         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17798         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17799                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17800         final var gravityNorm = gravity.getNorm();
17801 
17802         KnownBiasAndGravityNormAccelerometerCalibrator calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(
17803                 gravityNorm, measurements, true, ba, ma);
17804 
17805         // check default values
17806         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17807         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17808         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17809         final var bx1 = calibrator.getBiasXAsAcceleration();
17810         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17811         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17812         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17813         calibrator.getBiasXAsAcceleration(bx2);
17814         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17815         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17816         final var by1 = calibrator.getBiasYAsAcceleration();
17817         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17818         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17819         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17820         calibrator.getBiasYAsAcceleration(by2);
17821         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17822         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17823         final var bz1 = calibrator.getBiasZAsAcceleration();
17824         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17825         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17826         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17827         calibrator.getBiasZAsAcceleration(bz2);
17828         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17829         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17830         final var biasTriad1 = calibrator.getBiasAsTriad();
17831         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17832         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17833         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17834         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17835         final var biasTriad2 = new AccelerationTriad();
17836         calibrator.getBiasAsTriad(biasTriad2);
17837         assertEquals(biasTriad1, biasTriad2);
17838         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17839         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17840         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17841         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17842         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17843         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17844         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17845         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17846         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17847         final var bias1 = calibrator.getBias();
17848         assertArrayEquals(bias1, bias, 0.0);
17849         final var bias2 = new double[3];
17850         calibrator.getBias(bias2);
17851         assertArrayEquals(bias1, bias2, 0.0);
17852         final var b1 = calibrator.getBiasAsMatrix();
17853         assertEquals(b1, ba);
17854         final var b2 = new Matrix(3, 1);
17855         calibrator.getBiasAsMatrix(b2);
17856         assertEquals(b1, b2);
17857         final var ma1 = new Matrix(3, 3);
17858         ma1.setSubmatrix(0, 0, 2, 2,
17859                 new double[]{sx, myx, mzx,
17860                         mxy, sy, mzy,
17861                         mxz, myz, sz});
17862         assertEquals(ma1, calibrator.getInitialMa());
17863         final var ma2 = new Matrix(3, 3);
17864         calibrator.getInitialMa(ma2);
17865         assertEquals(ma1, ma2);
17866         assertSame(measurements, calibrator.getMeasurements());
17867         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
17868                 calibrator.getMeasurementType());
17869         assertFalse(calibrator.isOrderedMeasurementsRequired());
17870         assertFalse(calibrator.isQualityScoresRequired());
17871         assertTrue(calibrator.isCommonAxisUsed());
17872         assertNull(calibrator.getListener());
17873         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
17874         assertFalse(calibrator.isReady());
17875         assertFalse(calibrator.isRunning());
17876         assertNull(calibrator.getEstimatedMa());
17877         assertNull(calibrator.getEstimatedSx());
17878         assertNull(calibrator.getEstimatedSy());
17879         assertNull(calibrator.getEstimatedSz());
17880         assertNull(calibrator.getEstimatedMxy());
17881         assertNull(calibrator.getEstimatedMxz());
17882         assertNull(calibrator.getEstimatedMyx());
17883         assertNull(calibrator.getEstimatedMyz());
17884         assertNull(calibrator.getEstimatedMzx());
17885         assertNull(calibrator.getEstimatedMzy());
17886         assertNull(calibrator.getEstimatedCovariance());
17887         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
17888         assertNotNull(calibrator.getGroundTruthGravityNorm());
17889         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17890         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17891         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
17892                 ABSOLUTE_ERROR));
17893         final var g = new Acceleration(0.0, AccelerationUnit.G);
17894         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17895         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17896         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
17897 
17898         // Force IllegalArgumentException
17899         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17900                 -gravityNorm, measurements, true, ba, ma));
17901         final var m1 = new Matrix(1, 1);
17902         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17903                 gravityNorm, measurements, true, m1, ma));
17904         final var m2 = new Matrix(1, 3);
17905         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17906                 gravityNorm, measurements, true, m2, ma));
17907         final var m3 = new Matrix(1, 3);
17908         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17909                 gravityNorm, measurements, true, ba, m3));
17910         final var m4 = new Matrix(3, 1);
17911         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
17912                 gravityNorm, measurements, true, ba, m4));
17913     }
17914 
17915     @Test
17916     void testConstructor156() throws WrongSizeException {
17917         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
17918 
17919         final var ba = generateBa();
17920         final var bias = ba.getBuffer();
17921         final var biasX = ba.getElementAtIndex(0);
17922         final var biasY = ba.getElementAtIndex(1);
17923         final var biasZ = ba.getElementAtIndex(2);
17924 
17925         final var ma = generateMaCommonAxis();
17926         final var sx = ma.getElementAt(0, 0);
17927         final var sy = ma.getElementAt(1, 1);
17928         final var sz = ma.getElementAt(2, 2);
17929         final var mxy = ma.getElementAt(0, 1);
17930         final var mxz = ma.getElementAt(0, 2);
17931         final var myx = ma.getElementAt(1, 0);
17932         final var myz = ma.getElementAt(1, 2);
17933         final var mzx = ma.getElementAt(2, 0);
17934         final var mzy = ma.getElementAt(2, 1);
17935 
17936         final var randomizer = new UniformRandomizer();
17937         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
17938         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17939         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17940         final var nedPosition = new NEDPosition(latitude, longitude, height);
17941         final var nedVelocity = new NEDVelocity();
17942         final var ecefPosition = new ECEFPosition();
17943         final var ecefVelocity = new ECEFVelocity();
17944         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
17945         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17946                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17947         final var gravityNorm = gravity.getNorm();
17948 
17949         var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
17950                 true, ba, ma, this);
17951 
17952         // check default values
17953         assertEquals(biasX, calibrator.getBiasX(), 0.0);
17954         assertEquals(biasY, calibrator.getBiasY(), 0.0);
17955         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
17956         final var bx1 = calibrator.getBiasXAsAcceleration();
17957         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
17958         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
17959         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17960         calibrator.getBiasXAsAcceleration(bx2);
17961         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
17962         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
17963         final var by1 = calibrator.getBiasYAsAcceleration();
17964         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
17965         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
17966         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17967         calibrator.getBiasYAsAcceleration(by2);
17968         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
17969         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
17970         final var bz1 = calibrator.getBiasZAsAcceleration();
17971         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
17972         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
17973         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
17974         calibrator.getBiasZAsAcceleration(bz2);
17975         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
17976         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
17977         final var biasTriad1 = calibrator.getBiasAsTriad();
17978         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
17979         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
17980         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
17981         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
17982         final var biasTriad2 = new AccelerationTriad();
17983         calibrator.getBiasAsTriad(biasTriad2);
17984         assertEquals(biasTriad1, biasTriad2);
17985         assertEquals(sx, calibrator.getInitialSx(), 0.0);
17986         assertEquals(sy, calibrator.getInitialSy(), 0.0);
17987         assertEquals(sz, calibrator.getInitialSz(), 0.0);
17988         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
17989         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
17990         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
17991         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
17992         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
17993         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
17994         final var bias1 = calibrator.getBias();
17995         assertArrayEquals(bias1, bias, 0.0);
17996         final var bias2 = new double[3];
17997         calibrator.getBias(bias2);
17998         assertArrayEquals(bias1, bias2, 0.0);
17999         final var b1 = calibrator.getBiasAsMatrix();
18000         assertEquals(b1, ba);
18001         final var b2 = new Matrix(3, 1);
18002         calibrator.getBiasAsMatrix(b2);
18003         assertEquals(b1, b2);
18004         final var ma1 = new Matrix(3, 3);
18005         ma1.setSubmatrix(0, 0, 2, 2,
18006                 new double[]{sx, myx, mzx,
18007                         mxy, sy, mzy,
18008                         mxz, myz, sz});
18009         assertEquals(ma1, calibrator.getInitialMa());
18010         final var ma2 = new Matrix(3, 3);
18011         calibrator.getInitialMa(ma2);
18012         assertEquals(ma1, ma2);
18013         assertSame(measurements, calibrator.getMeasurements());
18014         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18015                 calibrator.getMeasurementType());
18016         assertFalse(calibrator.isOrderedMeasurementsRequired());
18017         assertFalse(calibrator.isQualityScoresRequired());
18018         assertTrue(calibrator.isCommonAxisUsed());
18019         assertSame(this, calibrator.getListener());
18020         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18021         assertFalse(calibrator.isReady());
18022         assertFalse(calibrator.isRunning());
18023         assertNull(calibrator.getEstimatedMa());
18024         assertNull(calibrator.getEstimatedSx());
18025         assertNull(calibrator.getEstimatedSy());
18026         assertNull(calibrator.getEstimatedSz());
18027         assertNull(calibrator.getEstimatedMxy());
18028         assertNull(calibrator.getEstimatedMxz());
18029         assertNull(calibrator.getEstimatedMyx());
18030         assertNull(calibrator.getEstimatedMyz());
18031         assertNull(calibrator.getEstimatedMzx());
18032         assertNull(calibrator.getEstimatedMzy());
18033         assertNull(calibrator.getEstimatedCovariance());
18034         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18035         assertNotNull(calibrator.getGroundTruthGravityNorm());
18036         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18037         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18038         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18039                 ABSOLUTE_ERROR));
18040         final var g = new Acceleration(0.0, AccelerationUnit.G);
18041         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18042         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18043         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18044 
18045         // Force IllegalArgumentException
18046         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18047                 -gravityNorm, measurements, true, ba, ma, this));
18048         final var m1 = new Matrix(1, 1);
18049         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18050                 gravityNorm, measurements, true, m1, ma, this));
18051         final var m2 = new Matrix(1, 3);
18052         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18053                 gravityNorm, measurements, true, m2, ma, this));
18054         final var m3 = new Matrix(1, 3);
18055         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18056                 gravityNorm, measurements, true, ba, m3, this));
18057         final var m4 = new Matrix(3, 1);
18058         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18059                 gravityNorm, measurements, true, ba, m4, this));
18060     }
18061 
18062     @Test
18063     void testConstructor157() throws WrongSizeException {
18064         final var randomizer = new UniformRandomizer();
18065         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18066         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18067         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18068         final var nedPosition = new NEDPosition(latitude, longitude, height);
18069         final var nedVelocity = new NEDVelocity();
18070         final var ecefPosition = new ECEFPosition();
18071         final var ecefVelocity = new ECEFVelocity();
18072         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18073         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18074                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18075         final var gravityNorm = gravity.getNormAsAcceleration();
18076 
18077         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm);
18078 
18079         // check default values
18080         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18081         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18082         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18083         final var bx1 = calibrator.getBiasXAsAcceleration();
18084         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18085         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18086         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18087         calibrator.getBiasXAsAcceleration(bx2);
18088         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18089         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18090         final var by1 = calibrator.getBiasYAsAcceleration();
18091         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18092         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18093         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18094         calibrator.getBiasYAsAcceleration(by2);
18095         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18096         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18097         final var bz1 = calibrator.getBiasZAsAcceleration();
18098         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18099         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18100         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18101         calibrator.getBiasZAsAcceleration(bz2);
18102         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18103         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18104         final var biasTriad1 = calibrator.getBiasAsTriad();
18105         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18106         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18107         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18108         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18109         final var biasTriad2 = new AccelerationTriad();
18110         calibrator.getBiasAsTriad(biasTriad2);
18111         assertEquals(biasTriad1, biasTriad2);
18112         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18113         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18114         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18115         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18116         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18117         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18118         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18119         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18120         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18121         final var bias1 = calibrator.getBias();
18122         assertArrayEquals(new double[3], bias1, 0.0);
18123         final var bias2 = new double[3];
18124         calibrator.getBias(bias2);
18125         assertArrayEquals(bias1, bias2, 0.0);
18126         final var b1 = calibrator.getBiasAsMatrix();
18127         assertEquals(new Matrix(3, 1), b1);
18128         final var b2 = new Matrix(3, 1);
18129         calibrator.getBiasAsMatrix(b2);
18130         assertEquals(b1, b2);
18131         final var ma1 = calibrator.getInitialMa();
18132         assertEquals(new Matrix(3, 3), ma1);
18133         final var ma2 = new Matrix(3, 3);
18134         calibrator.getInitialMa(ma2);
18135         assertEquals(ma1, ma2);
18136         assertNull(calibrator.getMeasurements());
18137         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18138                 calibrator.getMeasurementType());
18139         assertFalse(calibrator.isOrderedMeasurementsRequired());
18140         assertFalse(calibrator.isQualityScoresRequired());
18141         assertFalse(calibrator.isCommonAxisUsed());
18142         assertNull(calibrator.getListener());
18143         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18144         assertFalse(calibrator.isReady());
18145         assertFalse(calibrator.isRunning());
18146         assertNull(calibrator.getEstimatedMa());
18147         assertNull(calibrator.getEstimatedSx());
18148         assertNull(calibrator.getEstimatedSy());
18149         assertNull(calibrator.getEstimatedSz());
18150         assertNull(calibrator.getEstimatedMxy());
18151         assertNull(calibrator.getEstimatedMxz());
18152         assertNull(calibrator.getEstimatedMyx());
18153         assertNull(calibrator.getEstimatedMyz());
18154         assertNull(calibrator.getEstimatedMzx());
18155         assertNull(calibrator.getEstimatedMzy());
18156         assertNull(calibrator.getEstimatedCovariance());
18157         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18158         assertNotNull(calibrator.getGroundTruthGravityNorm());
18159         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18160         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18161         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18162                 ABSOLUTE_ERROR));
18163         final var g = new Acceleration(0.0, AccelerationUnit.G);
18164         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18165         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18166         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18167 
18168         // Force IllegalArgumentException
18169         final var invalidGravityNorm = new Acceleration(
18170                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18171 
18172         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18173                 invalidGravityNorm));
18174     }
18175 
18176     @Test
18177     void testConstructor158() throws WrongSizeException {
18178         final var randomizer = new UniformRandomizer();
18179         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18180         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18181         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18182         final var nedPosition = new NEDPosition(latitude, longitude, height);
18183         final var nedVelocity = new NEDVelocity();
18184         final var ecefPosition = new ECEFPosition();
18185         final var ecefVelocity = new ECEFVelocity();
18186         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18187         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18188                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18189         final var gravityNorm = gravity.getNormAsAcceleration();
18190 
18191         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, this);
18192 
18193         // check default values
18194         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18195         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18196         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18197         final var bx1 = calibrator.getBiasXAsAcceleration();
18198         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18199         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18200         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18201         calibrator.getBiasXAsAcceleration(bx2);
18202         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18203         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18204         final var by1 = calibrator.getBiasYAsAcceleration();
18205         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18206         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18207         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18208         calibrator.getBiasYAsAcceleration(by2);
18209         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18210         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18211         final var bz1 = calibrator.getBiasZAsAcceleration();
18212         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18213         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18214         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18215         calibrator.getBiasZAsAcceleration(bz2);
18216         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18217         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18218         final var biasTriad1 = calibrator.getBiasAsTriad();
18219         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18220         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18221         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18222         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18223         final var biasTriad2 = new AccelerationTriad();
18224         calibrator.getBiasAsTriad(biasTriad2);
18225         assertEquals(biasTriad1, biasTriad2);
18226         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18227         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18228         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18229         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18230         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18231         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18232         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18233         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18234         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18235         final var bias1 = calibrator.getBias();
18236         assertArrayEquals(new double[3], bias1, 0.0);
18237         final var bias2 = new double[3];
18238         calibrator.getBias(bias2);
18239         assertArrayEquals(bias1, bias2, 0.0);
18240         final var b1 = calibrator.getBiasAsMatrix();
18241         assertEquals(new Matrix(3, 1), b1);
18242         final var b2 = new Matrix(3, 1);
18243         calibrator.getBiasAsMatrix(b2);
18244         assertEquals(b1, b2);
18245         final var ma1 = calibrator.getInitialMa();
18246         assertEquals(new Matrix(3, 3), ma1);
18247         final var ma2 = new Matrix(3, 3);
18248         calibrator.getInitialMa(ma2);
18249         assertEquals(ma1, ma2);
18250         assertNull(calibrator.getMeasurements());
18251         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18252                 calibrator.getMeasurementType());
18253         assertFalse(calibrator.isOrderedMeasurementsRequired());
18254         assertFalse(calibrator.isQualityScoresRequired());
18255         assertFalse(calibrator.isCommonAxisUsed());
18256         assertSame(this, calibrator.getListener());
18257         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18258         assertFalse(calibrator.isReady());
18259         assertFalse(calibrator.isRunning());
18260         assertNull(calibrator.getEstimatedMa());
18261         assertNull(calibrator.getEstimatedSx());
18262         assertNull(calibrator.getEstimatedSy());
18263         assertNull(calibrator.getEstimatedSz());
18264         assertNull(calibrator.getEstimatedMxy());
18265         assertNull(calibrator.getEstimatedMxz());
18266         assertNull(calibrator.getEstimatedMyx());
18267         assertNull(calibrator.getEstimatedMyz());
18268         assertNull(calibrator.getEstimatedMzx());
18269         assertNull(calibrator.getEstimatedMzy());
18270         assertNull(calibrator.getEstimatedCovariance());
18271         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18272         assertNotNull(calibrator.getGroundTruthGravityNorm());
18273         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18274         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18275         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18276                 ABSOLUTE_ERROR));
18277         final var g = new Acceleration(0.0, AccelerationUnit.G);
18278         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18279         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18280         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18281 
18282         // Force IllegalArgumentException
18283         final var invalidGravityNorm = new Acceleration(
18284                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18285 
18286         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18287                 invalidGravityNorm, this));
18288     }
18289 
18290     @Test
18291     void testConstructor159() throws WrongSizeException {
18292         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18293 
18294         final var randomizer = new UniformRandomizer();
18295         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18296         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18297         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18298         final var nedPosition = new NEDPosition(latitude, longitude, height);
18299         final var nedVelocity = new NEDVelocity();
18300         final var ecefPosition = new ECEFPosition();
18301         final var ecefVelocity = new ECEFVelocity();
18302         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18303         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18304                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18305         final var gravityNorm = gravity.getNormAsAcceleration();
18306 
18307         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements);
18308 
18309         // check default values
18310         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18311         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18312         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18313         final var bx1 = calibrator.getBiasXAsAcceleration();
18314         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18315         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18316         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18317         calibrator.getBiasXAsAcceleration(bx2);
18318         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18319         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18320         final var by1 = calibrator.getBiasYAsAcceleration();
18321         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18322         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18323         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18324         calibrator.getBiasYAsAcceleration(by2);
18325         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18326         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18327         final var bz1 = calibrator.getBiasZAsAcceleration();
18328         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18329         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18330         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18331         calibrator.getBiasZAsAcceleration(bz2);
18332         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18333         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18334         final var biasTriad1 = calibrator.getBiasAsTriad();
18335         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18336         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18337         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18338         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18339         final var biasTriad2 = new AccelerationTriad();
18340         calibrator.getBiasAsTriad(biasTriad2);
18341         assertEquals(biasTriad1, biasTriad2);
18342         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18343         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18344         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18345         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18346         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18347         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18348         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18349         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18350         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18351         final var bias1 = calibrator.getBias();
18352         assertArrayEquals(new double[3], bias1, 0.0);
18353         final var bias2 = new double[3];
18354         calibrator.getBias(bias2);
18355         assertArrayEquals(bias1, bias2, 0.0);
18356         final var b1 = calibrator.getBiasAsMatrix();
18357         assertEquals(new Matrix(3, 1), b1);
18358         final var b2 = new Matrix(3, 1);
18359         calibrator.getBiasAsMatrix(b2);
18360         assertEquals(b1, b2);
18361         final var ma1 = calibrator.getInitialMa();
18362         assertEquals(new Matrix(3, 3), ma1);
18363         final var ma2 = new Matrix(3, 3);
18364         calibrator.getInitialMa(ma2);
18365         assertEquals(ma1, ma2);
18366         assertSame(measurements, calibrator.getMeasurements());
18367         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18368                 calibrator.getMeasurementType());
18369         assertFalse(calibrator.isOrderedMeasurementsRequired());
18370         assertFalse(calibrator.isQualityScoresRequired());
18371         assertFalse(calibrator.isCommonAxisUsed());
18372         assertNull(calibrator.getListener());
18373         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18374         assertFalse(calibrator.isReady());
18375         assertFalse(calibrator.isRunning());
18376         assertNull(calibrator.getEstimatedMa());
18377         assertNull(calibrator.getEstimatedSx());
18378         assertNull(calibrator.getEstimatedSy());
18379         assertNull(calibrator.getEstimatedSz());
18380         assertNull(calibrator.getEstimatedMxy());
18381         assertNull(calibrator.getEstimatedMxz());
18382         assertNull(calibrator.getEstimatedMyx());
18383         assertNull(calibrator.getEstimatedMyz());
18384         assertNull(calibrator.getEstimatedMzx());
18385         assertNull(calibrator.getEstimatedMzy());
18386         assertNull(calibrator.getEstimatedCovariance());
18387         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18388         assertNotNull(calibrator.getGroundTruthGravityNorm());
18389         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18390         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18391         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18392                 ABSOLUTE_ERROR));
18393         final var g = new Acceleration(0.0, AccelerationUnit.G);
18394         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18395         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18396         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18397 
18398         // Force IllegalArgumentException
18399         final var invalidGravityNorm = new Acceleration(
18400                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18401 
18402         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18403                 invalidGravityNorm, measurements));
18404     }
18405 
18406     @Test
18407     void testConstructor160() throws WrongSizeException {
18408         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18409 
18410         final var randomizer = new UniformRandomizer();
18411         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18412         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18413         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18414         final var nedPosition = new NEDPosition(latitude, longitude, height);
18415         final var nedVelocity = new NEDVelocity();
18416         final var ecefPosition = new ECEFPosition();
18417         final var ecefVelocity = new ECEFVelocity();
18418         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18419         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18420                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18421         final var gravityNorm = gravity.getNormAsAcceleration();
18422 
18423         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
18424                 this);
18425 
18426         // check default values
18427         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18428         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18429         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18430         final var bx1 = calibrator.getBiasXAsAcceleration();
18431         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18432         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18433         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18434         calibrator.getBiasXAsAcceleration(bx2);
18435         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18436         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18437         final var by1 = calibrator.getBiasYAsAcceleration();
18438         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18439         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18440         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18441         calibrator.getBiasYAsAcceleration(by2);
18442         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18443         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18444         final var bz1 = calibrator.getBiasZAsAcceleration();
18445         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18446         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18447         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18448         calibrator.getBiasZAsAcceleration(bz2);
18449         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18450         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18451         final var biasTriad1 = calibrator.getBiasAsTriad();
18452         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18453         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18454         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18455         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18456         final var biasTriad2 = new AccelerationTriad();
18457         calibrator.getBiasAsTriad(biasTriad2);
18458         assertEquals(biasTriad1, biasTriad2);
18459         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18460         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18461         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18462         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18463         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18464         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18465         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18466         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18467         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18468         final var bias1 = calibrator.getBias();
18469         assertArrayEquals(new double[3], bias1, 0.0);
18470         final var bias2 = new double[3];
18471         calibrator.getBias(bias2);
18472         assertArrayEquals(bias1, bias2, 0.0);
18473         final var b1 = calibrator.getBiasAsMatrix();
18474         assertEquals(new Matrix(3, 1), b1);
18475         final var b2 = new Matrix(3, 1);
18476         calibrator.getBiasAsMatrix(b2);
18477         assertEquals(b1, b2);
18478         final var ma1 = calibrator.getInitialMa();
18479         assertEquals(new Matrix(3, 3), ma1);
18480         final var ma2 = new Matrix(3, 3);
18481         calibrator.getInitialMa(ma2);
18482         assertEquals(ma1, ma2);
18483         assertSame(measurements, calibrator.getMeasurements());
18484         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18485                 calibrator.getMeasurementType());
18486         assertFalse(calibrator.isOrderedMeasurementsRequired());
18487         assertFalse(calibrator.isQualityScoresRequired());
18488         assertFalse(calibrator.isCommonAxisUsed());
18489         assertSame(this, calibrator.getListener());
18490         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
18491         assertFalse(calibrator.isReady());
18492         assertFalse(calibrator.isRunning());
18493         assertNull(calibrator.getEstimatedMa());
18494         assertNull(calibrator.getEstimatedSx());
18495         assertNull(calibrator.getEstimatedSy());
18496         assertNull(calibrator.getEstimatedSz());
18497         assertNull(calibrator.getEstimatedMxy());
18498         assertNull(calibrator.getEstimatedMxz());
18499         assertNull(calibrator.getEstimatedMyx());
18500         assertNull(calibrator.getEstimatedMyz());
18501         assertNull(calibrator.getEstimatedMzx());
18502         assertNull(calibrator.getEstimatedMzy());
18503         assertNull(calibrator.getEstimatedCovariance());
18504         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18505         assertNotNull(calibrator.getGroundTruthGravityNorm());
18506         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18507         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18508         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18509                 ABSOLUTE_ERROR));
18510         final var g = new Acceleration(0.0, AccelerationUnit.G);
18511         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18512         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18513         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18514 
18515         // Force IllegalArgumentException
18516         final var invalidGravityNorm = new Acceleration(
18517                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18518 
18519         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18520                 invalidGravityNorm, measurements, this));
18521     }
18522 
18523     @Test
18524     void testConstructor161() throws WrongSizeException {
18525         final var randomizer = new UniformRandomizer();
18526         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18527         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18528         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18529         final var nedPosition = new NEDPosition(latitude, longitude, height);
18530         final var nedVelocity = new NEDVelocity();
18531         final var ecefPosition = new ECEFPosition();
18532         final var ecefVelocity = new ECEFVelocity();
18533         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18534         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18535                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18536         final var gravityNorm = gravity.getNormAsAcceleration();
18537 
18538         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true);
18539 
18540         // check default values
18541         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18542         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18543         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18544         final var bx1 = calibrator.getBiasXAsAcceleration();
18545         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18546         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18547         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18548         calibrator.getBiasXAsAcceleration(bx2);
18549         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18550         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18551         final var by1 = calibrator.getBiasYAsAcceleration();
18552         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18553         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18554         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18555         calibrator.getBiasYAsAcceleration(by2);
18556         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18557         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18558         final var bz1 = calibrator.getBiasZAsAcceleration();
18559         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18560         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18561         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18562         calibrator.getBiasZAsAcceleration(bz2);
18563         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18564         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18565         final var biasTriad1 = calibrator.getBiasAsTriad();
18566         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18567         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18568         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18569         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18570         final var biasTriad2 = new AccelerationTriad();
18571         calibrator.getBiasAsTriad(biasTriad2);
18572         assertEquals(biasTriad1, biasTriad2);
18573         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18574         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18575         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18576         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18577         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18578         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18579         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18580         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18581         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18582         final var bias1 = calibrator.getBias();
18583         assertArrayEquals(new double[3], bias1, 0.0);
18584         final var bias2 = new double[3];
18585         calibrator.getBias(bias2);
18586         assertArrayEquals(bias1, bias2, 0.0);
18587         final var b1 = calibrator.getBiasAsMatrix();
18588         assertEquals(new Matrix(3, 1), b1);
18589         final var b2 = new Matrix(3, 1);
18590         calibrator.getBiasAsMatrix(b2);
18591         assertEquals(b1, b2);
18592         final var ma1 = calibrator.getInitialMa();
18593         assertEquals(new Matrix(3, 3), ma1);
18594         final var ma2 = new Matrix(3, 3);
18595         calibrator.getInitialMa(ma2);
18596         assertEquals(ma1, ma2);
18597         assertNull(calibrator.getMeasurements());
18598         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18599                 calibrator.getMeasurementType());
18600         assertFalse(calibrator.isOrderedMeasurementsRequired());
18601         assertFalse(calibrator.isQualityScoresRequired());
18602         assertTrue(calibrator.isCommonAxisUsed());
18603         assertNull(calibrator.getListener());
18604         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18605         assertFalse(calibrator.isReady());
18606         assertFalse(calibrator.isRunning());
18607         assertNull(calibrator.getEstimatedMa());
18608         assertNull(calibrator.getEstimatedSx());
18609         assertNull(calibrator.getEstimatedSy());
18610         assertNull(calibrator.getEstimatedSz());
18611         assertNull(calibrator.getEstimatedMxy());
18612         assertNull(calibrator.getEstimatedMxz());
18613         assertNull(calibrator.getEstimatedMyx());
18614         assertNull(calibrator.getEstimatedMyz());
18615         assertNull(calibrator.getEstimatedMzx());
18616         assertNull(calibrator.getEstimatedMzy());
18617         assertNull(calibrator.getEstimatedCovariance());
18618         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18619         assertNotNull(calibrator.getGroundTruthGravityNorm());
18620         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18621         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18622         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18623                 ABSOLUTE_ERROR));
18624         final var g = new Acceleration(0.0, AccelerationUnit.G);
18625         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18626         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18627         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18628 
18629         // Force IllegalArgumentException
18630         final var invalidGravityNorm = new Acceleration(
18631                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18632 
18633         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18634                 invalidGravityNorm, true));
18635     }
18636 
18637     @Test
18638     void testConstructor162() throws WrongSizeException {
18639         final var randomizer = new UniformRandomizer();
18640         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18641         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18642         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18643         final var nedPosition = new NEDPosition(latitude, longitude, height);
18644         final var nedVelocity = new NEDVelocity();
18645         final var ecefPosition = new ECEFPosition();
18646         final var ecefVelocity = new ECEFVelocity();
18647         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18648         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18649                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18650         final var gravityNorm = gravity.getNormAsAcceleration();
18651 
18652         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, 
18653                 this);
18654 
18655         // check default values
18656         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18657         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18658         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18659         final var bx1 = calibrator.getBiasXAsAcceleration();
18660         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18661         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18662         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18663         calibrator.getBiasXAsAcceleration(bx2);
18664         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18665         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18666         final var by1 = calibrator.getBiasYAsAcceleration();
18667         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18668         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18669         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18670         calibrator.getBiasYAsAcceleration(by2);
18671         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18672         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18673         final var bz1 = calibrator.getBiasZAsAcceleration();
18674         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18675         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18676         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18677         calibrator.getBiasZAsAcceleration(bz2);
18678         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18679         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18680         final var biasTriad1 = calibrator.getBiasAsTriad();
18681         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18682         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18683         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18684         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18685         final var biasTriad2 = new AccelerationTriad();
18686         calibrator.getBiasAsTriad(biasTriad2);
18687         assertEquals(biasTriad1, biasTriad2);
18688         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18689         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18690         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18691         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18692         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18693         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18694         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18695         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18696         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18697         final var bias1 = calibrator.getBias();
18698         assertArrayEquals(new double[3], bias1, 0.0);
18699         final var bias2 = new double[3];
18700         calibrator.getBias(bias2);
18701         assertArrayEquals(bias1, bias2, 0.0);
18702         final var b1 = calibrator.getBiasAsMatrix();
18703         assertEquals(new Matrix(3, 1), b1);
18704         final var b2 = new Matrix(3, 1);
18705         calibrator.getBiasAsMatrix(b2);
18706         assertEquals(b1, b2);
18707         final var ma1 = calibrator.getInitialMa();
18708         assertEquals(new Matrix(3, 3), ma1);
18709         final var ma2 = new Matrix(3, 3);
18710         calibrator.getInitialMa(ma2);
18711         assertEquals(ma1, ma2);
18712         assertNull(calibrator.getMeasurements());
18713         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18714                 calibrator.getMeasurementType());
18715         assertFalse(calibrator.isOrderedMeasurementsRequired());
18716         assertFalse(calibrator.isQualityScoresRequired());
18717         assertTrue(calibrator.isCommonAxisUsed());
18718         assertSame(this, calibrator.getListener());
18719         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18720         assertFalse(calibrator.isReady());
18721         assertFalse(calibrator.isRunning());
18722         assertNull(calibrator.getEstimatedMa());
18723         assertNull(calibrator.getEstimatedSx());
18724         assertNull(calibrator.getEstimatedSy());
18725         assertNull(calibrator.getEstimatedSz());
18726         assertNull(calibrator.getEstimatedMxy());
18727         assertNull(calibrator.getEstimatedMxz());
18728         assertNull(calibrator.getEstimatedMyx());
18729         assertNull(calibrator.getEstimatedMyz());
18730         assertNull(calibrator.getEstimatedMzx());
18731         assertNull(calibrator.getEstimatedMzy());
18732         assertNull(calibrator.getEstimatedCovariance());
18733         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18734         assertNotNull(calibrator.getGroundTruthGravityNorm());
18735         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18736         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18737         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18738                 ABSOLUTE_ERROR));
18739         final var g = new Acceleration(0.0, AccelerationUnit.G);
18740         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18741         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18742         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18743 
18744         // Force IllegalArgumentException
18745         final var invalidGravityNorm = new Acceleration(
18746                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18747 
18748         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18749                 invalidGravityNorm, true, this));
18750     }
18751 
18752     @Test
18753     void testConstructor163() throws WrongSizeException {
18754         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18755         final var randomizer = new UniformRandomizer();
18756         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18757         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18758         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18759         final var nedPosition = new NEDPosition(latitude, longitude, height);
18760         final var nedVelocity = new NEDVelocity();
18761         final var ecefPosition = new ECEFPosition();
18762         final var ecefVelocity = new ECEFVelocity();
18763         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18764         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18765                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18766         final var gravityNorm = gravity.getNormAsAcceleration();
18767 
18768         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
18769                 true);
18770 
18771         // check default values
18772         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18773         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18774         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18775         final var bx1 = calibrator.getBiasXAsAcceleration();
18776         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18777         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18778         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18779         calibrator.getBiasXAsAcceleration(bx2);
18780         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18781         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18782         final var by1 = calibrator.getBiasYAsAcceleration();
18783         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18784         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18785         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18786         calibrator.getBiasYAsAcceleration(by2);
18787         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18788         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18789         final var bz1 = calibrator.getBiasZAsAcceleration();
18790         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18791         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18792         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18793         calibrator.getBiasZAsAcceleration(bz2);
18794         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18795         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18796         final var biasTriad1 = calibrator.getBiasAsTriad();
18797         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18798         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18799         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18800         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18801         final var biasTriad2 = new AccelerationTriad();
18802         calibrator.getBiasAsTriad(biasTriad2);
18803         assertEquals(biasTriad1, biasTriad2);
18804         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18805         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18806         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18807         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18808         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18809         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18810         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18811         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18812         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18813         final var bias1 = calibrator.getBias();
18814         assertArrayEquals(new double[3], bias1, 0.0);
18815         final var bias2 = new double[3];
18816         calibrator.getBias(bias2);
18817         assertArrayEquals(bias1, bias2, 0.0);
18818         final var b1 = calibrator.getBiasAsMatrix();
18819         assertEquals(new Matrix(3, 1), b1);
18820         final var b2 = new Matrix(3, 1);
18821         calibrator.getBiasAsMatrix(b2);
18822         assertEquals(b1, b2);
18823         final var ma1 = calibrator.getInitialMa();
18824         assertEquals(new Matrix(3, 3), ma1);
18825         final var ma2 = new Matrix(3, 3);
18826         calibrator.getInitialMa(ma2);
18827         assertEquals(ma1, ma2);
18828         assertSame(measurements, calibrator.getMeasurements());
18829         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18830                 calibrator.getMeasurementType());
18831         assertFalse(calibrator.isOrderedMeasurementsRequired());
18832         assertFalse(calibrator.isQualityScoresRequired());
18833         assertTrue(calibrator.isCommonAxisUsed());
18834         assertNull(calibrator.getListener());
18835         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18836         assertFalse(calibrator.isReady());
18837         assertFalse(calibrator.isRunning());
18838         assertNull(calibrator.getEstimatedMa());
18839         assertNull(calibrator.getEstimatedSx());
18840         assertNull(calibrator.getEstimatedSy());
18841         assertNull(calibrator.getEstimatedSz());
18842         assertNull(calibrator.getEstimatedMxy());
18843         assertNull(calibrator.getEstimatedMxz());
18844         assertNull(calibrator.getEstimatedMyx());
18845         assertNull(calibrator.getEstimatedMyz());
18846         assertNull(calibrator.getEstimatedMzx());
18847         assertNull(calibrator.getEstimatedMzy());
18848         assertNull(calibrator.getEstimatedCovariance());
18849         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18850         assertNotNull(calibrator.getGroundTruthGravityNorm());
18851         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18852         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18853         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18854                 ABSOLUTE_ERROR));
18855         final var g = new Acceleration(0.0, AccelerationUnit.G);
18856         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18857         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18858         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18859 
18860         // Force IllegalArgumentException
18861         final var invalidGravityNorm = new Acceleration(
18862                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18863 
18864         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18865                 invalidGravityNorm, measurements, true));
18866     }
18867 
18868     @Test
18869     void testConstructor164() throws WrongSizeException {
18870         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
18871         final var randomizer = new UniformRandomizer();
18872         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18873         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18874         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18875         final var nedPosition = new NEDPosition(latitude, longitude, height);
18876         final var nedVelocity = new NEDVelocity();
18877         final var ecefPosition = new ECEFPosition();
18878         final var ecefVelocity = new ECEFVelocity();
18879         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
18880         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18881                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18882         final var gravityNorm = gravity.getNormAsAcceleration();
18883 
18884         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
18885                 true, this);
18886 
18887         // check default values
18888         assertEquals(0.0, calibrator.getBiasX(), 0.0);
18889         assertEquals(0.0, calibrator.getBiasY(), 0.0);
18890         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
18891         final var bx1 = calibrator.getBiasXAsAcceleration();
18892         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
18893         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
18894         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18895         calibrator.getBiasXAsAcceleration(bx2);
18896         assertEquals(0.0, bx2.getValue().doubleValue(), 0.0);
18897         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
18898         final var by1 = calibrator.getBiasYAsAcceleration();
18899         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
18900         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
18901         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18902         calibrator.getBiasYAsAcceleration(by2);
18903         assertEquals(0.0, by2.getValue().doubleValue(), 0.0);
18904         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
18905         final var bz1 = calibrator.getBiasZAsAcceleration();
18906         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
18907         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
18908         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
18909         calibrator.getBiasZAsAcceleration(bz2);
18910         assertEquals(0.0, bz2.getValue().doubleValue(), 0.0);
18911         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
18912         final var biasTriad1 = calibrator.getBiasAsTriad();
18913         assertEquals(0.0, biasTriad1.getValueX(), 0.0);
18914         assertEquals(0.0, biasTriad1.getValueY(), 0.0);
18915         assertEquals(0.0, biasTriad1.getValueZ(), 0.0);
18916         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
18917         final var biasTriad2 = new AccelerationTriad();
18918         calibrator.getBiasAsTriad(biasTriad2);
18919         assertEquals(biasTriad1, biasTriad2);
18920         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
18921         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
18922         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
18923         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
18924         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
18925         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
18926         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
18927         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
18928         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
18929         final var bias1 = calibrator.getBias();
18930         assertArrayEquals(new double[3], bias1, 0.0);
18931         final var bias2 = new double[3];
18932         calibrator.getBias(bias2);
18933         assertArrayEquals(bias1, bias2, 0.0);
18934         final var b1 = calibrator.getBiasAsMatrix();
18935         assertEquals(new Matrix(3, 1), b1);
18936         final var b2 = new Matrix(3, 1);
18937         calibrator.getBiasAsMatrix(b2);
18938         assertEquals(b1, b2);
18939         final var ma1 = calibrator.getInitialMa();
18940         assertEquals(new Matrix(3, 3), ma1);
18941         final var ma2 = new Matrix(3, 3);
18942         calibrator.getInitialMa(ma2);
18943         assertEquals(ma1, ma2);
18944         assertSame(measurements, calibrator.getMeasurements());
18945         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
18946                 calibrator.getMeasurementType());
18947         assertFalse(calibrator.isOrderedMeasurementsRequired());
18948         assertFalse(calibrator.isQualityScoresRequired());
18949         assertTrue(calibrator.isCommonAxisUsed());
18950         assertSame(this, calibrator.getListener());
18951         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
18952         assertFalse(calibrator.isReady());
18953         assertFalse(calibrator.isRunning());
18954         assertNull(calibrator.getEstimatedMa());
18955         assertNull(calibrator.getEstimatedSx());
18956         assertNull(calibrator.getEstimatedSy());
18957         assertNull(calibrator.getEstimatedSz());
18958         assertNull(calibrator.getEstimatedMxy());
18959         assertNull(calibrator.getEstimatedMxz());
18960         assertNull(calibrator.getEstimatedMyx());
18961         assertNull(calibrator.getEstimatedMyz());
18962         assertNull(calibrator.getEstimatedMzx());
18963         assertNull(calibrator.getEstimatedMzy());
18964         assertNull(calibrator.getEstimatedCovariance());
18965         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
18966         assertNotNull(calibrator.getGroundTruthGravityNorm());
18967         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18968         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18969         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
18970                 ABSOLUTE_ERROR));
18971         final var g = new Acceleration(0.0, AccelerationUnit.G);
18972         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18973         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18974         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
18975 
18976         // Force IllegalArgumentException
18977         final var invalidGravityNorm = new Acceleration(
18978                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18979 
18980         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
18981                 invalidGravityNorm, measurements, true, this));
18982     }
18983 
18984     @Test
18985     void testConstructor165() throws WrongSizeException {
18986         final var ba = generateBa();
18987         final var biasX = ba.getElementAtIndex(0);
18988         final var biasY = ba.getElementAtIndex(1);
18989         final var biasZ = ba.getElementAtIndex(2);
18990 
18991         final var randomizer = new UniformRandomizer();
18992         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
18993         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18994         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18995         final var nedPosition = new NEDPosition(latitude, longitude, height);
18996         final var nedVelocity = new NEDVelocity();
18997         final var ecefPosition = new ECEFPosition();
18998         final var ecefVelocity = new ECEFVelocity();
18999         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19000         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19001                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19002         final var gravityNorm = gravity.getNormAsAcceleration();
19003 
19004         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ);
19005 
19006         // check default values
19007         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19008         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19009         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19010         final var bx1 = calibrator.getBiasXAsAcceleration();
19011         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19012         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19013         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19014         calibrator.getBiasXAsAcceleration(bx2);
19015         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19016         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19017         final var by1 = calibrator.getBiasYAsAcceleration();
19018         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19019         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19020         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19021         calibrator.getBiasYAsAcceleration(by2);
19022         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19023         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19024         final var bz1 = calibrator.getBiasZAsAcceleration();
19025         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19026         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19027         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19028         calibrator.getBiasZAsAcceleration(bz2);
19029         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19030         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19031         final var biasTriad1 = calibrator.getBiasAsTriad();
19032         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19033         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19034         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19035         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19036         final var biasTriad2 = new AccelerationTriad();
19037         calibrator.getBiasAsTriad(biasTriad2);
19038         assertEquals(biasTriad1, biasTriad2);
19039         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19040         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19041         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19042         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19043         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19044         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19045         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19046         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19047         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19048         final var bias1 = calibrator.getBias();
19049         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19050         final var bias2 = new double[3];
19051         calibrator.getBias(bias2);
19052         assertArrayEquals(bias1, bias2, 0.0);
19053         final var b1 = calibrator.getBiasAsMatrix();
19054         assertEquals(b1, ba);
19055         final var b2 = new Matrix(3, 1);
19056         calibrator.getBiasAsMatrix(b2);
19057         assertEquals(b1, b2);
19058         final var ma1 = calibrator.getInitialMa();
19059         assertEquals(new Matrix(3, 3), ma1);
19060         final var ma2 = new Matrix(3, 3);
19061         calibrator.getInitialMa(ma2);
19062         assertEquals(ma1, ma2);
19063         assertNull(calibrator.getMeasurements());
19064         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19065                 calibrator.getMeasurementType());
19066         assertFalse(calibrator.isOrderedMeasurementsRequired());
19067         assertFalse(calibrator.isQualityScoresRequired());
19068         assertFalse(calibrator.isCommonAxisUsed());
19069         assertNull(calibrator.getListener());
19070         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19071         assertFalse(calibrator.isReady());
19072         assertFalse(calibrator.isRunning());
19073         assertNull(calibrator.getEstimatedMa());
19074         assertNull(calibrator.getEstimatedSx());
19075         assertNull(calibrator.getEstimatedSy());
19076         assertNull(calibrator.getEstimatedSz());
19077         assertNull(calibrator.getEstimatedMxy());
19078         assertNull(calibrator.getEstimatedMxz());
19079         assertNull(calibrator.getEstimatedMyx());
19080         assertNull(calibrator.getEstimatedMyz());
19081         assertNull(calibrator.getEstimatedMzx());
19082         assertNull(calibrator.getEstimatedMzy());
19083         assertNull(calibrator.getEstimatedCovariance());
19084         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19085         assertNotNull(calibrator.getGroundTruthGravityNorm());
19086         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19087         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19088         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19089                 ABSOLUTE_ERROR));
19090         final var g = new Acceleration(0.0, AccelerationUnit.G);
19091         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19092         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19093         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19094 
19095         // Force IllegalArgumentException
19096         final var invalidGravityNorm = new Acceleration(
19097                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19098 
19099         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19100                 invalidGravityNorm, biasX, biasY, biasZ));
19101     }
19102 
19103     @Test
19104     void testConstructor166() throws WrongSizeException {
19105         final var ba = generateBa();
19106         final var biasX = ba.getElementAtIndex(0);
19107         final var biasY = ba.getElementAtIndex(1);
19108         final var biasZ = ba.getElementAtIndex(2);
19109 
19110         final var randomizer = new UniformRandomizer();
19111         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19112         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19113         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19114         final var nedPosition = new NEDPosition(latitude, longitude, height);
19115         final var nedVelocity = new NEDVelocity();
19116         final var ecefPosition = new ECEFPosition();
19117         final var ecefVelocity = new ECEFVelocity();
19118         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19119         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19120                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19121         final var gravityNorm = gravity.getNormAsAcceleration();
19122 
19123         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ, 
19124                 this);
19125 
19126         // check default values
19127         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19128         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19129         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19130         final var bx1 = calibrator.getBiasXAsAcceleration();
19131         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19132         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19133         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19134         calibrator.getBiasXAsAcceleration(bx2);
19135         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19136         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19137         final var by1 = calibrator.getBiasYAsAcceleration();
19138         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19139         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19140         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19141         calibrator.getBiasYAsAcceleration(by2);
19142         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19143         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19144         final var bz1 = calibrator.getBiasZAsAcceleration();
19145         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19146         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19147         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19148         calibrator.getBiasZAsAcceleration(bz2);
19149         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19150         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19151         final var biasTriad1 = calibrator.getBiasAsTriad();
19152         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19153         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19154         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19155         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19156         final var biasTriad2 = new AccelerationTriad();
19157         calibrator.getBiasAsTriad(biasTriad2);
19158         assertEquals(biasTriad1, biasTriad2);
19159         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19160         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19161         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19162         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19163         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19164         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19165         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19166         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19167         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19168         final var bias1 = calibrator.getBias();
19169         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19170         final var bias2 = new double[3];
19171         calibrator.getBias(bias2);
19172         assertArrayEquals(bias1, bias2, 0.0);
19173         final var b1 = calibrator.getBiasAsMatrix();
19174         assertEquals(b1, ba);
19175         final var b2 = new Matrix(3, 1);
19176         calibrator.getBiasAsMatrix(b2);
19177         assertEquals(b1, b2);
19178         final var ma1 = calibrator.getInitialMa();
19179         assertEquals(new Matrix(3, 3), ma1);
19180         final var ma2 = new Matrix(3, 3);
19181         calibrator.getInitialMa(ma2);
19182         assertEquals(ma1, ma2);
19183         assertNull(calibrator.getMeasurements());
19184         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19185                 calibrator.getMeasurementType());
19186         assertFalse(calibrator.isOrderedMeasurementsRequired());
19187         assertFalse(calibrator.isQualityScoresRequired());
19188         assertFalse(calibrator.isCommonAxisUsed());
19189         assertSame(this, calibrator.getListener());
19190         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19191         assertFalse(calibrator.isReady());
19192         assertFalse(calibrator.isRunning());
19193         assertNull(calibrator.getEstimatedMa());
19194         assertNull(calibrator.getEstimatedSx());
19195         assertNull(calibrator.getEstimatedSy());
19196         assertNull(calibrator.getEstimatedSz());
19197         assertNull(calibrator.getEstimatedMxy());
19198         assertNull(calibrator.getEstimatedMxz());
19199         assertNull(calibrator.getEstimatedMyx());
19200         assertNull(calibrator.getEstimatedMyz());
19201         assertNull(calibrator.getEstimatedMzx());
19202         assertNull(calibrator.getEstimatedMzy());
19203         assertNull(calibrator.getEstimatedCovariance());
19204         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19205         assertNotNull(calibrator.getGroundTruthGravityNorm());
19206         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19207         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19208         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19209                 ABSOLUTE_ERROR));
19210         final var g = new Acceleration(0.0, AccelerationUnit.G);
19211         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19212         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19213         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19214 
19215         // Force IllegalArgumentException
19216         final var invalidGravityNorm = new Acceleration(
19217                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19218 
19219         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19220                 invalidGravityNorm, biasX, biasY, biasZ, this));
19221     }
19222 
19223     @Test
19224     void testConstructor167() throws WrongSizeException {
19225         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19226 
19227         final var ba = generateBa();
19228         final var biasX = ba.getElementAtIndex(0);
19229         final var biasY = ba.getElementAtIndex(1);
19230         final var biasZ = ba.getElementAtIndex(2);
19231 
19232         final var randomizer = new UniformRandomizer();
19233         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19234         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19235         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19236         final var nedPosition = new NEDPosition(latitude, longitude, height);
19237         final var nedVelocity = new NEDVelocity();
19238         final var ecefPosition = new ECEFPosition();
19239         final var ecefVelocity = new ECEFVelocity();
19240         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19241         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19242                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19243         final var gravityNorm = gravity.getNormAsAcceleration();
19244 
19245         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
19246                 biasX, biasY, biasZ);
19247 
19248         // check default values
19249         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19250         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19251         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19252         final var bx1 = calibrator.getBiasXAsAcceleration();
19253         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19254         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19255         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19256         calibrator.getBiasXAsAcceleration(bx2);
19257         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19258         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19259         final var by1 = calibrator.getBiasYAsAcceleration();
19260         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19261         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19262         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19263         calibrator.getBiasYAsAcceleration(by2);
19264         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19265         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19266         final var bz1 = calibrator.getBiasZAsAcceleration();
19267         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19268         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19269         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19270         calibrator.getBiasZAsAcceleration(bz2);
19271         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19272         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19273         final var biasTriad1 = calibrator.getBiasAsTriad();
19274         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19275         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19276         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19277         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19278         final var biasTriad2 = new AccelerationTriad();
19279         calibrator.getBiasAsTriad(biasTriad2);
19280         assertEquals(biasTriad1, biasTriad2);
19281         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19282         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19283         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19284         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19285         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19286         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19287         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19288         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19289         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19290         final var bias1 = calibrator.getBias();
19291         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19292         final var bias2 = new double[3];
19293         calibrator.getBias(bias2);
19294         assertArrayEquals(bias1, bias2, 0.0);
19295         final var b1 = calibrator.getBiasAsMatrix();
19296         assertEquals(b1, ba);
19297         final var b2 = new Matrix(3, 1);
19298         calibrator.getBiasAsMatrix(b2);
19299         assertEquals(b1, b2);
19300         final var ma1 = calibrator.getInitialMa();
19301         assertEquals(new Matrix(3, 3), ma1);
19302         final var ma2 = new Matrix(3, 3);
19303         calibrator.getInitialMa(ma2);
19304         assertEquals(ma1, ma2);
19305         assertSame(measurements, calibrator.getMeasurements());
19306         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19307                 calibrator.getMeasurementType());
19308         assertFalse(calibrator.isOrderedMeasurementsRequired());
19309         assertFalse(calibrator.isQualityScoresRequired());
19310         assertFalse(calibrator.isCommonAxisUsed());
19311         assertNull(calibrator.getListener());
19312         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19313         assertFalse(calibrator.isReady());
19314         assertFalse(calibrator.isRunning());
19315         assertNull(calibrator.getEstimatedMa());
19316         assertNull(calibrator.getEstimatedSx());
19317         assertNull(calibrator.getEstimatedSy());
19318         assertNull(calibrator.getEstimatedSz());
19319         assertNull(calibrator.getEstimatedMxy());
19320         assertNull(calibrator.getEstimatedMxz());
19321         assertNull(calibrator.getEstimatedMyx());
19322         assertNull(calibrator.getEstimatedMyz());
19323         assertNull(calibrator.getEstimatedMzx());
19324         assertNull(calibrator.getEstimatedMzy());
19325         assertNull(calibrator.getEstimatedCovariance());
19326         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19327         assertNotNull(calibrator.getGroundTruthGravityNorm());
19328         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19329         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19330         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19331                 ABSOLUTE_ERROR));
19332         final var g = new Acceleration(0.0, AccelerationUnit.G);
19333         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19334         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19335         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19336 
19337         // Force IllegalArgumentException
19338         final var invalidGravityNorm = new Acceleration(
19339                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19340 
19341         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19342                 invalidGravityNorm, measurements, biasX, biasY, biasZ));
19343     }
19344 
19345     @Test
19346     void testConstructor168() throws WrongSizeException {
19347         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19348 
19349         final var ba = generateBa();
19350         final var biasX = ba.getElementAtIndex(0);
19351         final var biasY = ba.getElementAtIndex(1);
19352         final var biasZ = ba.getElementAtIndex(2);
19353 
19354         final var randomizer = new UniformRandomizer();
19355         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19356         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19357         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19358         final var nedPosition = new NEDPosition(latitude, longitude, height);
19359         final var nedVelocity = new NEDVelocity();
19360         final var ecefPosition = new ECEFPosition();
19361         final var ecefVelocity = new ECEFVelocity();
19362         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19363         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19364                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19365         final var gravityNorm = gravity.getNormAsAcceleration();
19366 
19367         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
19368                 biasX, biasY, biasZ, this);
19369 
19370         // check default values
19371         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19372         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19373         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19374         final var bx1 = calibrator.getBiasXAsAcceleration();
19375         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19376         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19377         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19378         calibrator.getBiasXAsAcceleration(bx2);
19379         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19380         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19381         final var by1 = calibrator.getBiasYAsAcceleration();
19382         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19383         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19384         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19385         calibrator.getBiasYAsAcceleration(by2);
19386         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19387         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19388         final var bz1 = calibrator.getBiasZAsAcceleration();
19389         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19390         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19391         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19392         calibrator.getBiasZAsAcceleration(bz2);
19393         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19394         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19395         final var biasTriad1 = calibrator.getBiasAsTriad();
19396         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19397         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19398         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19399         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19400         final var biasTriad2 = new AccelerationTriad();
19401         calibrator.getBiasAsTriad(biasTriad2);
19402         assertEquals(biasTriad1, biasTriad2);
19403         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19404         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19405         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19406         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19407         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19408         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19409         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19410         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19411         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19412         final var bias1 = calibrator.getBias();
19413         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19414         final var bias2 = new double[3];
19415         calibrator.getBias(bias2);
19416         assertArrayEquals(bias1, bias2, 0.0);
19417         final var b1 = calibrator.getBiasAsMatrix();
19418         assertEquals(b1, ba);
19419         final var b2 = new Matrix(3, 1);
19420         calibrator.getBiasAsMatrix(b2);
19421         assertEquals(b1, b2);
19422         final var ma1 = calibrator.getInitialMa();
19423         assertEquals(new Matrix(3, 3), ma1);
19424         final var ma2 = new Matrix(3, 3);
19425         calibrator.getInitialMa(ma2);
19426         assertEquals(ma1, ma2);
19427         assertSame(measurements, calibrator.getMeasurements());
19428         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19429                 calibrator.getMeasurementType());
19430         assertFalse(calibrator.isOrderedMeasurementsRequired());
19431         assertFalse(calibrator.isQualityScoresRequired());
19432         assertFalse(calibrator.isCommonAxisUsed());
19433         assertSame(this, calibrator.getListener());
19434         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
19435         assertFalse(calibrator.isReady());
19436         assertFalse(calibrator.isRunning());
19437         assertNull(calibrator.getEstimatedMa());
19438         assertNull(calibrator.getEstimatedSx());
19439         assertNull(calibrator.getEstimatedSy());
19440         assertNull(calibrator.getEstimatedSz());
19441         assertNull(calibrator.getEstimatedMxy());
19442         assertNull(calibrator.getEstimatedMxz());
19443         assertNull(calibrator.getEstimatedMyx());
19444         assertNull(calibrator.getEstimatedMyz());
19445         assertNull(calibrator.getEstimatedMzx());
19446         assertNull(calibrator.getEstimatedMzy());
19447         assertNull(calibrator.getEstimatedCovariance());
19448         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19449         assertNotNull(calibrator.getGroundTruthGravityNorm());
19450         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19451         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19452         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19453                 ABSOLUTE_ERROR));
19454         final var g = new Acceleration(0.0, AccelerationUnit.G);
19455         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19456         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19457         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19458 
19459         // Force IllegalArgumentException
19460         final var invalidGravityNorm = new Acceleration(
19461                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19462 
19463         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19464                 invalidGravityNorm, measurements, biasX, biasY, biasZ, this));
19465     }
19466 
19467     @Test
19468     void testConstructor169() throws WrongSizeException {
19469         final var ba = generateBa();
19470         final var biasX = ba.getElementAtIndex(0);
19471         final var biasY = ba.getElementAtIndex(1);
19472         final var biasZ = ba.getElementAtIndex(2);
19473 
19474         final var randomizer = new UniformRandomizer();
19475         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19476         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19477         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19478         final var nedPosition = new NEDPosition(latitude, longitude, height);
19479         final var nedVelocity = new NEDVelocity();
19480         final var ecefPosition = new ECEFPosition();
19481         final var ecefVelocity = new ECEFVelocity();
19482         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19483         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19484                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19485         final var gravityNorm = gravity.getNormAsAcceleration();
19486 
19487         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, 
19488                 biasX, biasY, biasZ);
19489 
19490         // check default values
19491         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19492         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19493         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19494         final var bx1 = calibrator.getBiasXAsAcceleration();
19495         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19496         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19497         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19498         calibrator.getBiasXAsAcceleration(bx2);
19499         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19500         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19501         final var by1 = calibrator.getBiasYAsAcceleration();
19502         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19503         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19504         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19505         calibrator.getBiasYAsAcceleration(by2);
19506         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19507         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19508         final var bz1 = calibrator.getBiasZAsAcceleration();
19509         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19510         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19511         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19512         calibrator.getBiasZAsAcceleration(bz2);
19513         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19514         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19515         final var biasTriad1 = calibrator.getBiasAsTriad();
19516         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19517         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19518         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19519         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19520         final var biasTriad2 = new AccelerationTriad();
19521         calibrator.getBiasAsTriad(biasTriad2);
19522         assertEquals(biasTriad1, biasTriad2);
19523         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19524         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19525         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19526         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19527         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19528         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19529         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19530         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19531         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19532         final var bias1 = calibrator.getBias();
19533         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19534         final var bias2 = new double[3];
19535         calibrator.getBias(bias2);
19536         assertArrayEquals(bias1, bias2, 0.0);
19537         final var b1 = calibrator.getBiasAsMatrix();
19538         assertEquals(b1, ba);
19539         final var b2 = new Matrix(3, 1);
19540         calibrator.getBiasAsMatrix(b2);
19541         assertEquals(b1, b2);
19542         final var ma1 = calibrator.getInitialMa();
19543         assertEquals(new Matrix(3, 3), ma1);
19544         final var ma2 = new Matrix(3, 3);
19545         calibrator.getInitialMa(ma2);
19546         assertEquals(ma1, ma2);
19547         assertNull(calibrator.getMeasurements());
19548         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19549                 calibrator.getMeasurementType());
19550         assertFalse(calibrator.isOrderedMeasurementsRequired());
19551         assertFalse(calibrator.isQualityScoresRequired());
19552         assertTrue(calibrator.isCommonAxisUsed());
19553         assertNull(calibrator.getListener());
19554         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19555         assertFalse(calibrator.isReady());
19556         assertFalse(calibrator.isRunning());
19557         assertNull(calibrator.getEstimatedMa());
19558         assertNull(calibrator.getEstimatedSx());
19559         assertNull(calibrator.getEstimatedSy());
19560         assertNull(calibrator.getEstimatedSz());
19561         assertNull(calibrator.getEstimatedMxy());
19562         assertNull(calibrator.getEstimatedMxz());
19563         assertNull(calibrator.getEstimatedMyx());
19564         assertNull(calibrator.getEstimatedMyz());
19565         assertNull(calibrator.getEstimatedMzx());
19566         assertNull(calibrator.getEstimatedMzy());
19567         assertNull(calibrator.getEstimatedCovariance());
19568         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19569         assertNotNull(calibrator.getGroundTruthGravityNorm());
19570         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19571         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19572         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19573                 ABSOLUTE_ERROR));
19574         final var g = new Acceleration(0.0, AccelerationUnit.G);
19575         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19576         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19577         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19578 
19579         // Force IllegalArgumentException
19580         final var invalidGravityNorm = new Acceleration(
19581                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19582 
19583         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19584                 invalidGravityNorm, true, biasX, biasY, biasZ));
19585     }
19586 
19587     @Test
19588     void testConstructor170() throws WrongSizeException {
19589         final var ba = generateBa();
19590         final var biasX = ba.getElementAtIndex(0);
19591         final var biasY = ba.getElementAtIndex(1);
19592         final var biasZ = ba.getElementAtIndex(2);
19593 
19594         final var randomizer = new UniformRandomizer();
19595         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19596         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19597         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19598         final var nedPosition = new NEDPosition(latitude, longitude, height);
19599         final var nedVelocity = new NEDVelocity();
19600         final var ecefPosition = new ECEFPosition();
19601         final var ecefVelocity = new ECEFVelocity();
19602         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19603         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19604                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19605         final var gravityNorm = gravity.getNormAsAcceleration();
19606 
19607         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, 
19608                 biasX, biasY, biasZ, this);
19609 
19610         // check default values
19611         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19612         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19613         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19614         final var bx1 = calibrator.getBiasXAsAcceleration();
19615         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19616         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19617         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19618         calibrator.getBiasXAsAcceleration(bx2);
19619         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19620         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19621         final var by1 = calibrator.getBiasYAsAcceleration();
19622         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19623         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19624         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19625         calibrator.getBiasYAsAcceleration(by2);
19626         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19627         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19628         final var bz1 = calibrator.getBiasZAsAcceleration();
19629         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19630         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19631         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19632         calibrator.getBiasZAsAcceleration(bz2);
19633         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19634         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19635         final var biasTriad1 = calibrator.getBiasAsTriad();
19636         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19637         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19638         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19639         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19640         final var biasTriad2 = new AccelerationTriad();
19641         calibrator.getBiasAsTriad(biasTriad2);
19642         assertEquals(biasTriad1, biasTriad2);
19643         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19644         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19645         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19646         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19647         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19648         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19649         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19650         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19651         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19652         final var bias1 = calibrator.getBias();
19653         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19654         final var bias2 = new double[3];
19655         calibrator.getBias(bias2);
19656         assertArrayEquals(bias1, bias2, 0.0);
19657         final var b1 = calibrator.getBiasAsMatrix();
19658         assertEquals(b1, ba);
19659         final var b2 = new Matrix(3, 1);
19660         calibrator.getBiasAsMatrix(b2);
19661         assertEquals(b1, b2);
19662         final var ma1 = calibrator.getInitialMa();
19663         assertEquals(new Matrix(3, 3), ma1);
19664         final var ma2 = new Matrix(3, 3);
19665         calibrator.getInitialMa(ma2);
19666         assertEquals(ma1, ma2);
19667         assertNull(calibrator.getMeasurements());
19668         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19669                 calibrator.getMeasurementType());
19670         assertFalse(calibrator.isOrderedMeasurementsRequired());
19671         assertFalse(calibrator.isQualityScoresRequired());
19672         assertTrue(calibrator.isCommonAxisUsed());
19673         assertSame(this, calibrator.getListener());
19674         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19675         assertFalse(calibrator.isReady());
19676         assertFalse(calibrator.isRunning());
19677         assertNull(calibrator.getEstimatedMa());
19678         assertNull(calibrator.getEstimatedSx());
19679         assertNull(calibrator.getEstimatedSy());
19680         assertNull(calibrator.getEstimatedSz());
19681         assertNull(calibrator.getEstimatedMxy());
19682         assertNull(calibrator.getEstimatedMxz());
19683         assertNull(calibrator.getEstimatedMyx());
19684         assertNull(calibrator.getEstimatedMyz());
19685         assertNull(calibrator.getEstimatedMzx());
19686         assertNull(calibrator.getEstimatedMzy());
19687         assertNull(calibrator.getEstimatedCovariance());
19688         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19689         assertNotNull(calibrator.getGroundTruthGravityNorm());
19690         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19691         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19692         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19693                 ABSOLUTE_ERROR));
19694         final var g = new Acceleration(0.0, AccelerationUnit.G);
19695         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19696         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19697         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19698 
19699         // Force IllegalArgumentException
19700         final var invalidGravityNorm = new Acceleration(
19701                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19702 
19703         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19704                 invalidGravityNorm, true, biasX, biasY, biasZ, this));
19705     }
19706 
19707     @Test
19708     void testConstructor171() throws WrongSizeException {
19709         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19710 
19711         final var ba = generateBa();
19712         final var biasX = ba.getElementAtIndex(0);
19713         final var biasY = ba.getElementAtIndex(1);
19714         final var biasZ = ba.getElementAtIndex(2);
19715 
19716         final var randomizer = new UniformRandomizer();
19717         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19718         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19719         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19720         final var nedPosition = new NEDPosition(latitude, longitude, height);
19721         final var nedVelocity = new NEDVelocity();
19722         final var ecefPosition = new ECEFPosition();
19723         final var ecefVelocity = new ECEFVelocity();
19724         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19725         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19726                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19727         final var gravityNorm = gravity.getNormAsAcceleration();
19728 
19729         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, 
19730                 true, biasX, biasY, biasZ);
19731 
19732         // check default values
19733         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19734         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19735         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19736         final var bx1 = calibrator.getBiasXAsAcceleration();
19737         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19738         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19739         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19740         calibrator.getBiasXAsAcceleration(bx2);
19741         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19742         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19743         final var by1 = calibrator.getBiasYAsAcceleration();
19744         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19745         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19746         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19747         calibrator.getBiasYAsAcceleration(by2);
19748         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19749         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19750         final var bz1 = calibrator.getBiasZAsAcceleration();
19751         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19752         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19753         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19754         calibrator.getBiasZAsAcceleration(bz2);
19755         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19756         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19757         final var biasTriad1 = calibrator.getBiasAsTriad();
19758         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19759         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19760         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19761         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19762         final var biasTriad2 = new AccelerationTriad();
19763         calibrator.getBiasAsTriad(biasTriad2);
19764         assertEquals(biasTriad1, biasTriad2);
19765         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19766         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19767         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19768         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19769         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19770         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19771         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19772         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19773         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19774         final var bias1 = calibrator.getBias();
19775         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19776         final var bias2 = new double[3];
19777         calibrator.getBias(bias2);
19778         assertArrayEquals(bias1, bias2, 0.0);
19779         final var b1 = calibrator.getBiasAsMatrix();
19780         assertEquals(b1, ba);
19781         final var b2 = new Matrix(3, 1);
19782         calibrator.getBiasAsMatrix(b2);
19783         assertEquals(b1, b2);
19784         final var ma1 = calibrator.getInitialMa();
19785         assertEquals(new Matrix(3, 3), ma1);
19786         final var ma2 = new Matrix(3, 3);
19787         calibrator.getInitialMa(ma2);
19788         assertEquals(ma1, ma2);
19789         assertSame(measurements, calibrator.getMeasurements());
19790         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19791                 calibrator.getMeasurementType());
19792         assertFalse(calibrator.isOrderedMeasurementsRequired());
19793         assertFalse(calibrator.isQualityScoresRequired());
19794         assertTrue(calibrator.isCommonAxisUsed());
19795         assertNull(calibrator.getListener());
19796         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19797         assertFalse(calibrator.isReady());
19798         assertFalse(calibrator.isRunning());
19799         assertNull(calibrator.getEstimatedMa());
19800         assertNull(calibrator.getEstimatedSx());
19801         assertNull(calibrator.getEstimatedSy());
19802         assertNull(calibrator.getEstimatedSz());
19803         assertNull(calibrator.getEstimatedMxy());
19804         assertNull(calibrator.getEstimatedMxz());
19805         assertNull(calibrator.getEstimatedMyx());
19806         assertNull(calibrator.getEstimatedMyz());
19807         assertNull(calibrator.getEstimatedMzx());
19808         assertNull(calibrator.getEstimatedMzy());
19809         assertNull(calibrator.getEstimatedCovariance());
19810         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19811         assertNotNull(calibrator.getGroundTruthGravityNorm());
19812         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19813         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19814         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19815                 ABSOLUTE_ERROR));
19816         final var g = new Acceleration(0.0, AccelerationUnit.G);
19817         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19818         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19819         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19820 
19821         // Force IllegalArgumentException
19822         final var invalidGravityNorm = new Acceleration(
19823                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19824 
19825         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19826                 invalidGravityNorm, measurements, true, biasX, biasY, biasZ));
19827     }
19828 
19829     @Test
19830     void testConstructor172() throws WrongSizeException {
19831         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
19832 
19833         final var ba = generateBa();
19834         final var biasX = ba.getElementAtIndex(0);
19835         final var biasY = ba.getElementAtIndex(1);
19836         final var biasZ = ba.getElementAtIndex(2);
19837 
19838         final var randomizer = new UniformRandomizer();
19839         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19840         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19841         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19842         final var nedPosition = new NEDPosition(latitude, longitude, height);
19843         final var nedVelocity = new NEDVelocity();
19844         final var ecefPosition = new ECEFPosition();
19845         final var ecefVelocity = new ECEFVelocity();
19846         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19847         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19848                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19849         final var gravityNorm = gravity.getNormAsAcceleration();
19850 
19851         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, true,
19852                 biasX, biasY, biasZ, this);
19853 
19854         // check default values
19855         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19856         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19857         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19858         final var bx1 = calibrator.getBiasXAsAcceleration();
19859         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19860         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19861         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19862         calibrator.getBiasXAsAcceleration(bx2);
19863         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19864         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19865         final var by1 = calibrator.getBiasYAsAcceleration();
19866         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19867         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19868         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19869         calibrator.getBiasYAsAcceleration(by2);
19870         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19871         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19872         final var bz1 = calibrator.getBiasZAsAcceleration();
19873         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19874         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19875         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19876         calibrator.getBiasZAsAcceleration(bz2);
19877         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
19878         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
19879         final var biasTriad1 = calibrator.getBiasAsTriad();
19880         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
19881         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
19882         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
19883         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
19884         final var biasTriad2 = new AccelerationTriad();
19885         calibrator.getBiasAsTriad(biasTriad2);
19886         assertEquals(biasTriad1, biasTriad2);
19887         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
19888         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
19889         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
19890         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
19891         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
19892         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
19893         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
19894         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
19895         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
19896         final var bias1 = calibrator.getBias();
19897         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
19898         final var bias2 = new double[3];
19899         calibrator.getBias(bias2);
19900         assertArrayEquals(bias1, bias2, 0.0);
19901         final var b1 = calibrator.getBiasAsMatrix();
19902         assertEquals(b1, ba);
19903         final var b2 = new Matrix(3, 1);
19904         calibrator.getBiasAsMatrix(b2);
19905         assertEquals(b1, b2);
19906         final var ma1 = calibrator.getInitialMa();
19907         assertEquals(new Matrix(3, 3), ma1);
19908         final var ma2 = new Matrix(3, 3);
19909         calibrator.getInitialMa(ma2);
19910         assertEquals(ma1, ma2);
19911         assertSame(measurements, calibrator.getMeasurements());
19912         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
19913                 calibrator.getMeasurementType());
19914         assertFalse(calibrator.isOrderedMeasurementsRequired());
19915         assertFalse(calibrator.isQualityScoresRequired());
19916         assertTrue(calibrator.isCommonAxisUsed());
19917         assertSame(this, calibrator.getListener());
19918         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
19919         assertFalse(calibrator.isReady());
19920         assertFalse(calibrator.isRunning());
19921         assertNull(calibrator.getEstimatedMa());
19922         assertNull(calibrator.getEstimatedSx());
19923         assertNull(calibrator.getEstimatedSy());
19924         assertNull(calibrator.getEstimatedSz());
19925         assertNull(calibrator.getEstimatedMxy());
19926         assertNull(calibrator.getEstimatedMxz());
19927         assertNull(calibrator.getEstimatedMyx());
19928         assertNull(calibrator.getEstimatedMyz());
19929         assertNull(calibrator.getEstimatedMzx());
19930         assertNull(calibrator.getEstimatedMzy());
19931         assertNull(calibrator.getEstimatedCovariance());
19932         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
19933         assertNotNull(calibrator.getGroundTruthGravityNorm());
19934         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19935         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19936         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
19937                 ABSOLUTE_ERROR));
19938         final var g = new Acceleration(0.0, AccelerationUnit.G);
19939         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19940         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19941         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
19942 
19943         // Force IllegalArgumentException
19944         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19945 
19946         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
19947                 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, this));
19948     }
19949 
19950     @Test
19951     void testConstructor173() throws WrongSizeException {
19952         final var ba = generateBa();
19953         final var biasX = ba.getElementAtIndex(0);
19954         final var biasY = ba.getElementAtIndex(1);
19955         final var biasZ = ba.getElementAtIndex(2);
19956 
19957         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
19958         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
19959         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
19960 
19961         final var randomizer = new UniformRandomizer();
19962         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
19963         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19964         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19965         final var nedPosition = new NEDPosition(latitude, longitude, height);
19966         final var nedVelocity = new NEDVelocity();
19967         final var ecefPosition = new ECEFPosition();
19968         final var ecefVelocity = new ECEFVelocity();
19969         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
19970         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19971                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19972         final var gravityNorm = gravity.getNormAsAcceleration();
19973 
19974         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz);
19975 
19976         // check default values
19977         assertEquals(biasX, calibrator.getBiasX(), 0.0);
19978         assertEquals(biasY, calibrator.getBiasY(), 0.0);
19979         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
19980         final var bx1 = calibrator.getBiasXAsAcceleration();
19981         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
19982         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
19983         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19984         calibrator.getBiasXAsAcceleration(bx2);
19985         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
19986         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
19987         final var by1 = calibrator.getBiasYAsAcceleration();
19988         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
19989         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
19990         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19991         calibrator.getBiasYAsAcceleration(by2);
19992         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
19993         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
19994         final var bz1 = calibrator.getBiasZAsAcceleration();
19995         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
19996         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
19997         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
19998         calibrator.getBiasZAsAcceleration(bz2);
19999         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20000         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20001         final var biasTriad1 = calibrator.getBiasAsTriad();
20002         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20003         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20004         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20005         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20006         final var biasTriad2 = new AccelerationTriad();
20007         calibrator.getBiasAsTriad(biasTriad2);
20008         assertEquals(biasTriad1, biasTriad2);
20009         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20010         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20011         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20012         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20013         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20014         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20015         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20016         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20017         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20018         final var bias1 = calibrator.getBias();
20019         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20020         final var bias2 = new double[3];
20021         calibrator.getBias(bias2);
20022         assertArrayEquals(bias1, bias2, 0.0);
20023         final var b1 = calibrator.getBiasAsMatrix();
20024         assertEquals(b1, ba);
20025         final var b2 = new Matrix(3, 1);
20026         calibrator.getBiasAsMatrix(b2);
20027         assertEquals(b1, b2);
20028         final var ma1 = calibrator.getInitialMa();
20029         assertEquals(new Matrix(3, 3), ma1);
20030         final var ma2 = new Matrix(3, 3);
20031         calibrator.getInitialMa(ma2);
20032         assertEquals(ma1, ma2);
20033         assertNull(calibrator.getMeasurements());
20034         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20035                 calibrator.getMeasurementType());
20036         assertFalse(calibrator.isOrderedMeasurementsRequired());
20037         assertFalse(calibrator.isQualityScoresRequired());
20038         assertFalse(calibrator.isCommonAxisUsed());
20039         assertNull(calibrator.getListener());
20040         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20041         assertFalse(calibrator.isReady());
20042         assertFalse(calibrator.isRunning());
20043         assertNull(calibrator.getEstimatedMa());
20044         assertNull(calibrator.getEstimatedSx());
20045         assertNull(calibrator.getEstimatedSy());
20046         assertNull(calibrator.getEstimatedSz());
20047         assertNull(calibrator.getEstimatedMxy());
20048         assertNull(calibrator.getEstimatedMxz());
20049         assertNull(calibrator.getEstimatedMyx());
20050         assertNull(calibrator.getEstimatedMyz());
20051         assertNull(calibrator.getEstimatedMzx());
20052         assertNull(calibrator.getEstimatedMzy());
20053         assertNull(calibrator.getEstimatedCovariance());
20054         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20055         assertNotNull(calibrator.getGroundTruthGravityNorm());
20056         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20057         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20058         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20059                 ABSOLUTE_ERROR));
20060         final var g = new Acceleration(0.0, AccelerationUnit.G);
20061         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20062         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20063         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20064 
20065         // Force IllegalArgumentException
20066         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20067 
20068         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20069                 invalidGravityNorm, bx, by, bz));
20070     }
20071 
20072     @Test
20073     void testConstructor174() throws WrongSizeException {
20074         final var ba = generateBa();
20075         final var biasX = ba.getElementAtIndex(0);
20076         final var biasY = ba.getElementAtIndex(1);
20077         final var biasZ = ba.getElementAtIndex(2);
20078 
20079         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20080         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20081         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20082 
20083         final var randomizer = new UniformRandomizer();
20084         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20085         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20086         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20087         final var nedPosition = new NEDPosition(latitude, longitude, height);
20088         final var nedVelocity = new NEDVelocity();
20089         final var ecefPosition = new ECEFPosition();
20090         final var ecefVelocity = new ECEFVelocity();
20091         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20092         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20093                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20094         final var gravityNorm = gravity.getNormAsAcceleration();
20095 
20096         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz,
20097                 this);
20098 
20099         // check default values
20100         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20101         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20102         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20103         final var bx1 = calibrator.getBiasXAsAcceleration();
20104         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20105         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20106         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20107         calibrator.getBiasXAsAcceleration(bx2);
20108         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20109         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20110         final var by1 = calibrator.getBiasYAsAcceleration();
20111         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20112         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20113         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20114         calibrator.getBiasYAsAcceleration(by2);
20115         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20116         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20117         final var bz1 = calibrator.getBiasZAsAcceleration();
20118         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20119         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20120         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20121         calibrator.getBiasZAsAcceleration(bz2);
20122         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20123         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20124         final var biasTriad1 = calibrator.getBiasAsTriad();
20125         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20126         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20127         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20128         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20129         final var biasTriad2 = new AccelerationTriad();
20130         calibrator.getBiasAsTriad(biasTriad2);
20131         assertEquals(biasTriad1, biasTriad2);
20132         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20133         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20134         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20135         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20136         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20137         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20138         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20139         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20140         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20141         final var bias1 = calibrator.getBias();
20142         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20143         final var bias2 = new double[3];
20144         calibrator.getBias(bias2);
20145         assertArrayEquals(bias1, bias2, 0.0);
20146         final var b1 = calibrator.getBiasAsMatrix();
20147         assertEquals(b1, ba);
20148         final var b2 = new Matrix(3, 1);
20149         calibrator.getBiasAsMatrix(b2);
20150         assertEquals(b1, b2);
20151         final var ma1 = calibrator.getInitialMa();
20152         assertEquals(new Matrix(3, 3), ma1);
20153         final var ma2 = new Matrix(3, 3);
20154         calibrator.getInitialMa(ma2);
20155         assertEquals(ma1, ma2);
20156         assertNull(calibrator.getMeasurements());
20157         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20158                 calibrator.getMeasurementType());
20159         assertFalse(calibrator.isOrderedMeasurementsRequired());
20160         assertFalse(calibrator.isQualityScoresRequired());
20161         assertFalse(calibrator.isCommonAxisUsed());
20162         assertSame(this, calibrator.getListener());
20163         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20164         assertFalse(calibrator.isReady());
20165         assertFalse(calibrator.isRunning());
20166         assertNull(calibrator.getEstimatedMa());
20167         assertNull(calibrator.getEstimatedSx());
20168         assertNull(calibrator.getEstimatedSy());
20169         assertNull(calibrator.getEstimatedSz());
20170         assertNull(calibrator.getEstimatedMxy());
20171         assertNull(calibrator.getEstimatedMxz());
20172         assertNull(calibrator.getEstimatedMyx());
20173         assertNull(calibrator.getEstimatedMyz());
20174         assertNull(calibrator.getEstimatedMzx());
20175         assertNull(calibrator.getEstimatedMzy());
20176         assertNull(calibrator.getEstimatedCovariance());
20177         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20178         assertNotNull(calibrator.getGroundTruthGravityNorm());
20179         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20180         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20181         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20182                 ABSOLUTE_ERROR));
20183         final var g = new Acceleration(0.0, AccelerationUnit.G);
20184         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20185         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20186         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20187 
20188         // Force IllegalArgumentException
20189         final var invalidGravityNorm = new Acceleration(
20190                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20191 
20192         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20193                 invalidGravityNorm, bx, by, bz, this));
20194     }
20195 
20196     @Test
20197     void testConstructor175() throws WrongSizeException {
20198         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20199 
20200         final var ba = generateBa();
20201         final var biasX = ba.getElementAtIndex(0);
20202         final var biasY = ba.getElementAtIndex(1);
20203         final var biasZ = ba.getElementAtIndex(2);
20204 
20205         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20206         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20207         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20208 
20209         final var randomizer = new UniformRandomizer();
20210         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20211         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20212         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20213         final var nedPosition = new NEDPosition(latitude, longitude, height);
20214         final var nedVelocity = new NEDVelocity();
20215         final var ecefPosition = new ECEFPosition();
20216         final var ecefVelocity = new ECEFVelocity();
20217         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20218         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20219                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20220         final var gravityNorm = gravity.getNormAsAcceleration();
20221 
20222         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
20223                 bx, by, bz);
20224 
20225         // check default values
20226         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20227         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20228         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20229         final var bx1 = calibrator.getBiasXAsAcceleration();
20230         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20231         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20232         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20233         calibrator.getBiasXAsAcceleration(bx2);
20234         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20235         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20236         final var by1 = calibrator.getBiasYAsAcceleration();
20237         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20238         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20239         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20240         calibrator.getBiasYAsAcceleration(by2);
20241         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20242         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20243         final var bz1 = calibrator.getBiasZAsAcceleration();
20244         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20245         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20246         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20247         calibrator.getBiasZAsAcceleration(bz2);
20248         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20249         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20250         final var biasTriad1 = calibrator.getBiasAsTriad();
20251         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20252         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20253         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20254         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20255         final var biasTriad2 = new AccelerationTriad();
20256         calibrator.getBiasAsTriad(biasTriad2);
20257         assertEquals(biasTriad1, biasTriad2);
20258         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20259         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20260         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20261         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20262         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20263         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20264         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20265         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20266         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20267         final var bias1 = calibrator.getBias();
20268         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20269         final var bias2 = new double[3];
20270         calibrator.getBias(bias2);
20271         assertArrayEquals(bias1, bias2, 0.0);
20272         final var b1 = calibrator.getBiasAsMatrix();
20273         assertEquals(b1, ba);
20274         final var b2 = new Matrix(3, 1);
20275         calibrator.getBiasAsMatrix(b2);
20276         assertEquals(b1, b2);
20277         final var ma1 = calibrator.getInitialMa();
20278         assertEquals(new Matrix(3, 3), ma1);
20279         final var ma2 = new Matrix(3, 3);
20280         calibrator.getInitialMa(ma2);
20281         assertEquals(ma1, ma2);
20282         assertSame(measurements, calibrator.getMeasurements());
20283         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20284                 calibrator.getMeasurementType());
20285         assertFalse(calibrator.isOrderedMeasurementsRequired());
20286         assertFalse(calibrator.isQualityScoresRequired());
20287         assertFalse(calibrator.isCommonAxisUsed());
20288         assertNull(calibrator.getListener());
20289         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20290         assertFalse(calibrator.isReady());
20291         assertFalse(calibrator.isRunning());
20292         assertNull(calibrator.getEstimatedMa());
20293         assertNull(calibrator.getEstimatedSx());
20294         assertNull(calibrator.getEstimatedSy());
20295         assertNull(calibrator.getEstimatedSz());
20296         assertNull(calibrator.getEstimatedMxy());
20297         assertNull(calibrator.getEstimatedMxz());
20298         assertNull(calibrator.getEstimatedMyx());
20299         assertNull(calibrator.getEstimatedMyz());
20300         assertNull(calibrator.getEstimatedMzx());
20301         assertNull(calibrator.getEstimatedMzy());
20302         assertNull(calibrator.getEstimatedCovariance());
20303         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20304         assertNotNull(calibrator.getGroundTruthGravityNorm());
20305         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20306         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20307         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20308                 ABSOLUTE_ERROR));
20309         final var g = new Acceleration(0.0, AccelerationUnit.G);
20310         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20311         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20312         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20313 
20314         // Force IllegalArgumentException
20315         final var invalidGravityNorm = new Acceleration(
20316                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20317 
20318         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20319                 invalidGravityNorm, measurements, bx, by, bz));
20320     }
20321 
20322     @Test
20323     void testConstructor176() throws WrongSizeException {
20324         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20325 
20326         final var ba = generateBa();
20327         final var biasX = ba.getElementAtIndex(0);
20328         final var biasY = ba.getElementAtIndex(1);
20329         final var biasZ = ba.getElementAtIndex(2);
20330 
20331         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20332         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20333         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20334 
20335         final var randomizer = new UniformRandomizer();
20336         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20337         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20338         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20339         final var nedPosition = new NEDPosition(latitude, longitude, height);
20340         final var nedVelocity = new NEDVelocity();
20341         final var ecefPosition = new ECEFPosition();
20342         final var ecefVelocity = new ECEFVelocity();
20343         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20344         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20345                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20346         final var gravityNorm = gravity.getNormAsAcceleration();
20347 
20348         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
20349                 this);
20350 
20351         // check default values
20352         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20353         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20354         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20355         final var bx1 = calibrator.getBiasXAsAcceleration();
20356         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20357         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20358         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20359         calibrator.getBiasXAsAcceleration(bx2);
20360         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20361         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20362         final var by1 = calibrator.getBiasYAsAcceleration();
20363         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20364         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20365         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20366         calibrator.getBiasYAsAcceleration(by2);
20367         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20368         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20369         final var bz1 = calibrator.getBiasZAsAcceleration();
20370         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20371         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20372         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20373         calibrator.getBiasZAsAcceleration(bz2);
20374         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20375         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20376         final var biasTriad1 = calibrator.getBiasAsTriad();
20377         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20378         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20379         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20380         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20381         final var biasTriad2 = new AccelerationTriad();
20382         calibrator.getBiasAsTriad(biasTriad2);
20383         assertEquals(biasTriad1, biasTriad2);
20384         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20385         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20386         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20387         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20388         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20389         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20390         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20391         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20392         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20393         final var bias1 = calibrator.getBias();
20394         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20395         final var bias2 = new double[3];
20396         calibrator.getBias(bias2);
20397         assertArrayEquals(bias1, bias2, 0.0);
20398         final var b1 = calibrator.getBiasAsMatrix();
20399         assertEquals(b1, ba);
20400         final var b2 = new Matrix(3, 1);
20401         calibrator.getBiasAsMatrix(b2);
20402         assertEquals(b1, b2);
20403         final var ma1 = calibrator.getInitialMa();
20404         assertEquals(new Matrix(3, 3), ma1);
20405         final var ma2 = new Matrix(3, 3);
20406         calibrator.getInitialMa(ma2);
20407         assertEquals(ma1, ma2);
20408         assertSame(measurements, calibrator.getMeasurements());
20409         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20410                 calibrator.getMeasurementType());
20411         assertFalse(calibrator.isOrderedMeasurementsRequired());
20412         assertFalse(calibrator.isQualityScoresRequired());
20413         assertFalse(calibrator.isCommonAxisUsed());
20414         assertSame(this, calibrator.getListener());
20415         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
20416         assertFalse(calibrator.isReady());
20417         assertFalse(calibrator.isRunning());
20418         assertNull(calibrator.getEstimatedMa());
20419         assertNull(calibrator.getEstimatedSx());
20420         assertNull(calibrator.getEstimatedSy());
20421         assertNull(calibrator.getEstimatedSz());
20422         assertNull(calibrator.getEstimatedMxy());
20423         assertNull(calibrator.getEstimatedMxz());
20424         assertNull(calibrator.getEstimatedMyx());
20425         assertNull(calibrator.getEstimatedMyz());
20426         assertNull(calibrator.getEstimatedMzx());
20427         assertNull(calibrator.getEstimatedMzy());
20428         assertNull(calibrator.getEstimatedCovariance());
20429         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20430         assertNotNull(calibrator.getGroundTruthGravityNorm());
20431         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20432         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20433         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20434                 ABSOLUTE_ERROR));
20435         final var g = new Acceleration(0.0, AccelerationUnit.G);
20436         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20437         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20438         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20439 
20440         // Force IllegalArgumentException
20441         final var invalidGravityNorm = new Acceleration(
20442                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20443 
20444         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20445                 invalidGravityNorm, measurements, bx, by, bz, this));
20446     }
20447 
20448     @Test
20449     void testConstructor177() throws WrongSizeException {
20450         final var ba = generateBa();
20451         final var biasX = ba.getElementAtIndex(0);
20452         final var biasY = ba.getElementAtIndex(1);
20453         final var biasZ = ba.getElementAtIndex(2);
20454 
20455         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20456         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20457         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20458 
20459         final var randomizer = new UniformRandomizer();
20460         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20461         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20462         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20463         final var nedPosition = new NEDPosition(latitude, longitude, height);
20464         final var nedVelocity = new NEDVelocity();
20465         final var ecefPosition = new ECEFPosition();
20466         final var ecefVelocity = new ECEFVelocity();
20467         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20468         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20469                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20470         final var gravityNorm = gravity.getNormAsAcceleration();
20471 
20472         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
20473                 bx, by, bz);
20474 
20475         // check default values
20476         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20477         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20478         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20479         final var bx1 = calibrator.getBiasXAsAcceleration();
20480         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20481         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20482         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20483         calibrator.getBiasXAsAcceleration(bx2);
20484         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20485         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20486         final var by1 = calibrator.getBiasYAsAcceleration();
20487         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20488         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20489         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20490         calibrator.getBiasYAsAcceleration(by2);
20491         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20492         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20493         final var bz1 = calibrator.getBiasZAsAcceleration();
20494         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20495         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20496         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20497         calibrator.getBiasZAsAcceleration(bz2);
20498         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20499         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20500         final var biasTriad1 = calibrator.getBiasAsTriad();
20501         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20502         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20503         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20504         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20505         final var biasTriad2 = new AccelerationTriad();
20506         calibrator.getBiasAsTriad(biasTriad2);
20507         assertEquals(biasTriad1, biasTriad2);
20508         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20509         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20510         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20511         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20512         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20513         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20514         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20515         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20516         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20517         final var bias1 = calibrator.getBias();
20518         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20519         final var bias2 = new double[3];
20520         calibrator.getBias(bias2);
20521         assertArrayEquals(bias1, bias2, 0.0);
20522         final var b1 = calibrator.getBiasAsMatrix();
20523         assertEquals(b1, ba);
20524         final var b2 = new Matrix(3, 1);
20525         calibrator.getBiasAsMatrix(b2);
20526         assertEquals(b1, b2);
20527         final var ma1 = calibrator.getInitialMa();
20528         assertEquals(new Matrix(3, 3), ma1);
20529         final var ma2 = new Matrix(3, 3);
20530         calibrator.getInitialMa(ma2);
20531         assertEquals(ma1, ma2);
20532         assertNull(calibrator.getMeasurements());
20533         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20534                 calibrator.getMeasurementType());
20535         assertFalse(calibrator.isOrderedMeasurementsRequired());
20536         assertFalse(calibrator.isQualityScoresRequired());
20537         assertTrue(calibrator.isCommonAxisUsed());
20538         assertNull(calibrator.getListener());
20539         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20540         assertFalse(calibrator.isReady());
20541         assertFalse(calibrator.isRunning());
20542         assertNull(calibrator.getEstimatedMa());
20543         assertNull(calibrator.getEstimatedSx());
20544         assertNull(calibrator.getEstimatedSy());
20545         assertNull(calibrator.getEstimatedSz());
20546         assertNull(calibrator.getEstimatedMxy());
20547         assertNull(calibrator.getEstimatedMxz());
20548         assertNull(calibrator.getEstimatedMyx());
20549         assertNull(calibrator.getEstimatedMyz());
20550         assertNull(calibrator.getEstimatedMzx());
20551         assertNull(calibrator.getEstimatedMzy());
20552         assertNull(calibrator.getEstimatedCovariance());
20553         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20554         assertNotNull(calibrator.getGroundTruthGravityNorm());
20555         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20556         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20557         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20558                 ABSOLUTE_ERROR));
20559         final var g = new Acceleration(0.0, AccelerationUnit.G);
20560         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20561         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20562         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20563 
20564         // Force IllegalArgumentException
20565         final var invalidGravityNorm = new Acceleration(
20566                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20567 
20568         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20569                 invalidGravityNorm, true, bx, by, bz));
20570     }
20571 
20572     @Test
20573     void testConstructor178() throws WrongSizeException {
20574         final var ba = generateBa();
20575         final var biasX = ba.getElementAtIndex(0);
20576         final var biasY = ba.getElementAtIndex(1);
20577         final var biasZ = ba.getElementAtIndex(2);
20578 
20579         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20580         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20581         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20582 
20583         final var randomizer = new UniformRandomizer();
20584         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20585         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20586         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20587         final var nedPosition = new NEDPosition(latitude, longitude, height);
20588         final var nedVelocity = new NEDVelocity();
20589         final var ecefPosition = new ECEFPosition();
20590         final var ecefVelocity = new ECEFVelocity();
20591         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20592         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20593                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20594         final var gravityNorm = gravity.getNormAsAcceleration();
20595 
20596         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
20597                 bx, by, bz, this);
20598 
20599         // check default values
20600         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20601         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20602         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20603         final var bx1 = calibrator.getBiasXAsAcceleration();
20604         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20605         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20606         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20607         calibrator.getBiasXAsAcceleration(bx2);
20608         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20609         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20610         final var by1 = calibrator.getBiasYAsAcceleration();
20611         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20612         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20613         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20614         calibrator.getBiasYAsAcceleration(by2);
20615         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20616         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20617         final var bz1 = calibrator.getBiasZAsAcceleration();
20618         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20619         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20620         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20621         calibrator.getBiasZAsAcceleration(bz2);
20622         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20623         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20624         final var biasTriad1 = calibrator.getBiasAsTriad();
20625         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20626         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20627         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20628         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20629         final var biasTriad2 = new AccelerationTriad();
20630         calibrator.getBiasAsTriad(biasTriad2);
20631         assertEquals(biasTriad1, biasTriad2);
20632         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20633         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20634         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20635         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20636         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20637         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20638         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20639         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20640         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20641         final var bias1 = calibrator.getBias();
20642         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20643         final var bias2 = new double[3];
20644         calibrator.getBias(bias2);
20645         assertArrayEquals(bias1, bias2, 0.0);
20646         final var b1 = calibrator.getBiasAsMatrix();
20647         assertEquals(b1, ba);
20648         final var b2 = new Matrix(3, 1);
20649         calibrator.getBiasAsMatrix(b2);
20650         assertEquals(b1, b2);
20651         final var ma1 = calibrator.getInitialMa();
20652         assertEquals(new Matrix(3, 3), ma1);
20653         final var ma2 = new Matrix(3, 3);
20654         calibrator.getInitialMa(ma2);
20655         assertEquals(ma1, ma2);
20656         assertNull(calibrator.getMeasurements());
20657         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20658                 calibrator.getMeasurementType());
20659         assertFalse(calibrator.isOrderedMeasurementsRequired());
20660         assertFalse(calibrator.isQualityScoresRequired());
20661         assertTrue(calibrator.isCommonAxisUsed());
20662         assertSame(this, calibrator.getListener());
20663         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20664         assertFalse(calibrator.isReady());
20665         assertFalse(calibrator.isRunning());
20666         assertNull(calibrator.getEstimatedMa());
20667         assertNull(calibrator.getEstimatedSx());
20668         assertNull(calibrator.getEstimatedSy());
20669         assertNull(calibrator.getEstimatedSz());
20670         assertNull(calibrator.getEstimatedMxy());
20671         assertNull(calibrator.getEstimatedMxz());
20672         assertNull(calibrator.getEstimatedMyx());
20673         assertNull(calibrator.getEstimatedMyz());
20674         assertNull(calibrator.getEstimatedMzx());
20675         assertNull(calibrator.getEstimatedMzy());
20676         assertNull(calibrator.getEstimatedCovariance());
20677         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20678         assertNotNull(calibrator.getGroundTruthGravityNorm());
20679         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20680         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20681         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20682                 ABSOLUTE_ERROR));
20683         final var g = new Acceleration(0.0, AccelerationUnit.G);
20684         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20685         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20686         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20687 
20688         // Force IllegalArgumentException
20689         final var invalidGravityNorm = new Acceleration(
20690                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20691 
20692         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20693                 invalidGravityNorm, true, bx, by, bz, this));
20694     }
20695 
20696     @Test
20697     void testConstructor179() throws WrongSizeException {
20698         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20699 
20700         final var ba = generateBa();
20701         final var biasX = ba.getElementAtIndex(0);
20702         final var biasY = ba.getElementAtIndex(1);
20703         final var biasZ = ba.getElementAtIndex(2);
20704 
20705         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20706         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20707         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20708 
20709         final var randomizer = new UniformRandomizer();
20710         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20711         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20712         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20713         final var nedPosition = new NEDPosition(latitude, longitude, height);
20714         final var nedVelocity = new NEDVelocity();
20715         final var ecefPosition = new ECEFPosition();
20716         final var ecefVelocity = new ECEFVelocity();
20717         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20718         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20719                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20720         final var gravityNorm = gravity.getNormAsAcceleration();
20721 
20722         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
20723                 true, bx, by, bz);
20724 
20725         // check default values
20726         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20727         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20728         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20729         final var bx1 = calibrator.getBiasXAsAcceleration();
20730         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20731         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20732         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20733         calibrator.getBiasXAsAcceleration(bx2);
20734         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20735         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20736         final var by1 = calibrator.getBiasYAsAcceleration();
20737         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20738         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20739         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20740         calibrator.getBiasYAsAcceleration(by2);
20741         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20742         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20743         final var bz1 = calibrator.getBiasZAsAcceleration();
20744         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20745         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20746         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20747         calibrator.getBiasZAsAcceleration(bz2);
20748         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20749         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20750         final var biasTriad1 = calibrator.getBiasAsTriad();
20751         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20752         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20753         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20754         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20755         final var biasTriad2 = new AccelerationTriad();
20756         calibrator.getBiasAsTriad(biasTriad2);
20757         assertEquals(biasTriad1, biasTriad2);
20758         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20759         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20760         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20761         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20762         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20763         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20764         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20765         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20766         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20767         final var bias1 = calibrator.getBias();
20768         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20769         final var bias2 = new double[3];
20770         calibrator.getBias(bias2);
20771         assertArrayEquals(bias1, bias2, 0.0);
20772         final var b1 = calibrator.getBiasAsMatrix();
20773         assertEquals(b1, ba);
20774         final var b2 = new Matrix(3, 1);
20775         calibrator.getBiasAsMatrix(b2);
20776         assertEquals(b1, b2);
20777         final var ma1 = calibrator.getInitialMa();
20778         assertEquals(new Matrix(3, 3), ma1);
20779         final var ma2 = new Matrix(3, 3);
20780         calibrator.getInitialMa(ma2);
20781         assertEquals(ma1, ma2);
20782         assertSame(measurements, calibrator.getMeasurements());
20783         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20784                 calibrator.getMeasurementType());
20785         assertFalse(calibrator.isOrderedMeasurementsRequired());
20786         assertFalse(calibrator.isQualityScoresRequired());
20787         assertTrue(calibrator.isCommonAxisUsed());
20788         assertNull(calibrator.getListener());
20789         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20790         assertFalse(calibrator.isReady());
20791         assertFalse(calibrator.isRunning());
20792         assertNull(calibrator.getEstimatedMa());
20793         assertNull(calibrator.getEstimatedSx());
20794         assertNull(calibrator.getEstimatedSy());
20795         assertNull(calibrator.getEstimatedSz());
20796         assertNull(calibrator.getEstimatedMxy());
20797         assertNull(calibrator.getEstimatedMxz());
20798         assertNull(calibrator.getEstimatedMyx());
20799         assertNull(calibrator.getEstimatedMyz());
20800         assertNull(calibrator.getEstimatedMzx());
20801         assertNull(calibrator.getEstimatedMzy());
20802         assertNull(calibrator.getEstimatedCovariance());
20803         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20804         assertNotNull(calibrator.getGroundTruthGravityNorm());
20805         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20806         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20807         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20808                 ABSOLUTE_ERROR));
20809         final var g = new Acceleration(0.0, AccelerationUnit.G);
20810         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20811         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20812         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20813 
20814         // Force IllegalArgumentException
20815         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20816 
20817         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20818                 invalidGravityNorm, measurements, true, bx, by, bz));
20819     }
20820 
20821     @Test
20822     void testConstructor180() throws WrongSizeException {
20823         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
20824 
20825         final var ba = generateBa();
20826         final var biasX = ba.getElementAtIndex(0);
20827         final var biasY = ba.getElementAtIndex(1);
20828         final var biasZ = ba.getElementAtIndex(2);
20829 
20830         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20831         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20832         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
20833 
20834         final var randomizer = new UniformRandomizer();
20835         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20836         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20837         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20838         final var nedPosition = new NEDPosition(latitude, longitude, height);
20839         final var nedVelocity = new NEDVelocity();
20840         final var ecefPosition = new ECEFPosition();
20841         final var ecefVelocity = new ECEFVelocity();
20842         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20843         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20844                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20845         final var gravityNorm = gravity.getNormAsAcceleration();
20846 
20847         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
20848                 true, bx, by, bz, this);
20849 
20850         // check default values
20851         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20852         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20853         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20854         final var bx1 = calibrator.getBiasXAsAcceleration();
20855         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20856         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20857         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20858         calibrator.getBiasXAsAcceleration(bx2);
20859         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20860         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20861         final var by1 = calibrator.getBiasYAsAcceleration();
20862         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20863         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20864         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20865         calibrator.getBiasYAsAcceleration(by2);
20866         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20867         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20868         final var bz1 = calibrator.getBiasZAsAcceleration();
20869         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20870         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20871         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20872         calibrator.getBiasZAsAcceleration(bz2);
20873         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20874         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
20875         final var biasTriad1 = calibrator.getBiasAsTriad();
20876         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
20877         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
20878         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
20879         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
20880         final var biasTriad2 = new AccelerationTriad();
20881         calibrator.getBiasAsTriad(biasTriad2);
20882         assertEquals(biasTriad1, biasTriad2);
20883         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
20884         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
20885         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
20886         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
20887         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
20888         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
20889         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
20890         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
20891         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
20892         final var bias1 = calibrator.getBias();
20893         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
20894         final var bias2 = new double[3];
20895         calibrator.getBias(bias2);
20896         assertArrayEquals(bias1, bias2, 0.0);
20897         final var b1 = calibrator.getBiasAsMatrix();
20898         assertEquals(b1, ba);
20899         final var b2 = new Matrix(3, 1);
20900         calibrator.getBiasAsMatrix(b2);
20901         assertEquals(b1, b2);
20902         final var ma1 = calibrator.getInitialMa();
20903         assertEquals(new Matrix(3, 3), ma1);
20904         final var ma2 = new Matrix(3, 3);
20905         calibrator.getInitialMa(ma2);
20906         assertEquals(ma1, ma2);
20907         assertSame(measurements, calibrator.getMeasurements());
20908         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
20909                 calibrator.getMeasurementType());
20910         assertFalse(calibrator.isOrderedMeasurementsRequired());
20911         assertFalse(calibrator.isQualityScoresRequired());
20912         assertTrue(calibrator.isCommonAxisUsed());
20913         assertSame(this, calibrator.getListener());
20914         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
20915         assertFalse(calibrator.isReady());
20916         assertFalse(calibrator.isRunning());
20917         assertNull(calibrator.getEstimatedMa());
20918         assertNull(calibrator.getEstimatedSx());
20919         assertNull(calibrator.getEstimatedSy());
20920         assertNull(calibrator.getEstimatedSz());
20921         assertNull(calibrator.getEstimatedMxy());
20922         assertNull(calibrator.getEstimatedMxz());
20923         assertNull(calibrator.getEstimatedMyx());
20924         assertNull(calibrator.getEstimatedMyz());
20925         assertNull(calibrator.getEstimatedMzx());
20926         assertNull(calibrator.getEstimatedMzy());
20927         assertNull(calibrator.getEstimatedCovariance());
20928         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
20929         assertNotNull(calibrator.getGroundTruthGravityNorm());
20930         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20931         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20932         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
20933                 ABSOLUTE_ERROR));
20934         final var g = new Acceleration(0.0, AccelerationUnit.G);
20935         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20936         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20937         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
20938 
20939         // Force IllegalArgumentException
20940         final var invalidGravityNorm = new Acceleration(
20941                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20942 
20943         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
20944                 invalidGravityNorm, measurements, true, bx, by, bz, this));
20945     }
20946 
20947     @Test
20948     void testConstructor181() throws WrongSizeException {
20949         final var ba = generateBa();
20950         final var biasX = ba.getElementAtIndex(0);
20951         final var biasY = ba.getElementAtIndex(1);
20952         final var biasZ = ba.getElementAtIndex(2);
20953 
20954         final var ma = generateMaCommonAxis();
20955         final var sx = ma.getElementAt(0, 0);
20956         final var sy = ma.getElementAt(1, 1);
20957         final var sz = ma.getElementAt(2, 2);
20958 
20959         final var randomizer = new UniformRandomizer();
20960         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
20961         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20962         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20963         final var nedPosition = new NEDPosition(latitude, longitude, height);
20964         final var nedVelocity = new NEDVelocity();
20965         final var ecefPosition = new ECEFPosition();
20966         final var ecefVelocity = new ECEFVelocity();
20967         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
20968         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20969                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20970         final var gravityNorm = gravity.getNormAsAcceleration();
20971 
20972         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
20973                 sx, sy, sz);
20974 
20975         // check default values
20976         assertEquals(biasX, calibrator.getBiasX(), 0.0);
20977         assertEquals(biasY, calibrator.getBiasY(), 0.0);
20978         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
20979         final var bx1 = calibrator.getBiasXAsAcceleration();
20980         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
20981         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
20982         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20983         calibrator.getBiasXAsAcceleration(bx2);
20984         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
20985         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
20986         final var by1 = calibrator.getBiasYAsAcceleration();
20987         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
20988         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
20989         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20990         calibrator.getBiasYAsAcceleration(by2);
20991         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
20992         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
20993         final var bz1 = calibrator.getBiasZAsAcceleration();
20994         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
20995         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
20996         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
20997         calibrator.getBiasZAsAcceleration(bz2);
20998         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
20999         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21000         final var biasTriad1 = calibrator.getBiasAsTriad();
21001         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21002         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21003         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21004         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21005         final var biasTriad2 = new AccelerationTriad();
21006         calibrator.getBiasAsTriad(biasTriad2);
21007         assertEquals(biasTriad1, biasTriad2);
21008         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21009         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21010         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21011         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21012         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21013         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21014         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21015         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21016         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21017         final var bias1 = calibrator.getBias();
21018         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21019         final var bias2 = new double[3];
21020         calibrator.getBias(bias2);
21021         assertArrayEquals(bias1, bias2, 0.0);
21022         final var b1 = calibrator.getBiasAsMatrix();
21023         assertEquals(b1, ba);
21024         final var b2 = new Matrix(3, 1);
21025         calibrator.getBiasAsMatrix(b2);
21026         assertEquals(b1, b2);
21027         final var ma1 = calibrator.getInitialMa();
21028         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21029         final var ma2 = new Matrix(3, 3);
21030         calibrator.getInitialMa(ma2);
21031         assertEquals(ma1, ma2);
21032         assertNull(calibrator.getMeasurements());
21033         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21034                 calibrator.getMeasurementType());
21035         assertFalse(calibrator.isOrderedMeasurementsRequired());
21036         assertFalse(calibrator.isQualityScoresRequired());
21037         assertFalse(calibrator.isCommonAxisUsed());
21038         assertNull(calibrator.getListener());
21039         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21040         assertFalse(calibrator.isReady());
21041         assertFalse(calibrator.isRunning());
21042         assertNull(calibrator.getEstimatedMa());
21043         assertNull(calibrator.getEstimatedSx());
21044         assertNull(calibrator.getEstimatedSy());
21045         assertNull(calibrator.getEstimatedSz());
21046         assertNull(calibrator.getEstimatedMxy());
21047         assertNull(calibrator.getEstimatedMxz());
21048         assertNull(calibrator.getEstimatedMyx());
21049         assertNull(calibrator.getEstimatedMyz());
21050         assertNull(calibrator.getEstimatedMzx());
21051         assertNull(calibrator.getEstimatedMzy());
21052         assertNull(calibrator.getEstimatedCovariance());
21053         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21054         assertNotNull(calibrator.getGroundTruthGravityNorm());
21055         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21056         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21057         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21058                 ABSOLUTE_ERROR));
21059         final var g = new Acceleration(0.0, AccelerationUnit.G);
21060         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21061         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21062         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21063 
21064         // Force IllegalArgumentException
21065         final var invalidGravityNorm = new Acceleration(
21066                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21067 
21068         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21069                 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz));
21070     }
21071 
21072     @Test
21073     void testConstructor182() throws WrongSizeException {
21074         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21075 
21076         final var ba = generateBa();
21077         final var biasX = ba.getElementAtIndex(0);
21078         final var biasY = ba.getElementAtIndex(1);
21079         final var biasZ = ba.getElementAtIndex(2);
21080 
21081         final var ma = generateMaCommonAxis();
21082         final var sx = ma.getElementAt(0, 0);
21083         final var sy = ma.getElementAt(1, 1);
21084         final var sz = ma.getElementAt(2, 2);
21085 
21086         final var randomizer = new UniformRandomizer();
21087         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21088         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21089         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21090         final var nedPosition = new NEDPosition(latitude, longitude, height);
21091         final var nedVelocity = new NEDVelocity();
21092         final var ecefPosition = new ECEFPosition();
21093         final var ecefVelocity = new ECEFVelocity();
21094         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21095         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21096                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21097         final var gravityNorm = gravity.getNormAsAcceleration();
21098 
21099         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21100                 biasX, biasY, biasZ, sx, sy, sz);
21101 
21102         // check default values
21103         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21104         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21105         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21106         final var bx1 = calibrator.getBiasXAsAcceleration();
21107         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21108         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21109         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21110         calibrator.getBiasXAsAcceleration(bx2);
21111         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21112         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21113         final var by1 = calibrator.getBiasYAsAcceleration();
21114         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21115         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21116         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21117         calibrator.getBiasYAsAcceleration(by2);
21118         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21119         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21120         final var bz1 = calibrator.getBiasZAsAcceleration();
21121         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21122         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21123         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21124         calibrator.getBiasZAsAcceleration(bz2);
21125         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21126         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21127         final var biasTriad1 = calibrator.getBiasAsTriad();
21128         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21129         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21130         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21131         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21132         final var biasTriad2 = new AccelerationTriad();
21133         calibrator.getBiasAsTriad(biasTriad2);
21134         assertEquals(biasTriad1, biasTriad2);
21135         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21136         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21137         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21138         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21139         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21140         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21141         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21142         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21143         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21144         final var bias1 = calibrator.getBias();
21145         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21146         final var bias2 = new double[3];
21147         calibrator.getBias(bias2);
21148         assertArrayEquals(bias1, bias2, 0.0);
21149         final var b1 = calibrator.getBiasAsMatrix();
21150         assertEquals(b1, ba);
21151         final var b2 = new Matrix(3, 1);
21152         calibrator.getBiasAsMatrix(b2);
21153         assertEquals(b1, b2);
21154         final var ma1 = calibrator.getInitialMa();
21155         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21156         final var ma2 = new Matrix(3, 3);
21157         calibrator.getInitialMa(ma2);
21158         assertEquals(ma1, ma2);
21159         assertSame(measurements, calibrator.getMeasurements());
21160         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21161                 calibrator.getMeasurementType());
21162         assertFalse(calibrator.isOrderedMeasurementsRequired());
21163         assertFalse(calibrator.isQualityScoresRequired());
21164         assertFalse(calibrator.isCommonAxisUsed());
21165         assertNull(calibrator.getListener());
21166         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21167         assertFalse(calibrator.isReady());
21168         assertFalse(calibrator.isRunning());
21169         assertNull(calibrator.getEstimatedMa());
21170         assertNull(calibrator.getEstimatedSx());
21171         assertNull(calibrator.getEstimatedSy());
21172         assertNull(calibrator.getEstimatedSz());
21173         assertNull(calibrator.getEstimatedMxy());
21174         assertNull(calibrator.getEstimatedMxz());
21175         assertNull(calibrator.getEstimatedMyx());
21176         assertNull(calibrator.getEstimatedMyz());
21177         assertNull(calibrator.getEstimatedMzx());
21178         assertNull(calibrator.getEstimatedMzy());
21179         assertNull(calibrator.getEstimatedCovariance());
21180         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21181         assertNotNull(calibrator.getGroundTruthGravityNorm());
21182         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21183         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21184         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21185                 ABSOLUTE_ERROR));
21186         final var g = new Acceleration(0.0, AccelerationUnit.G);
21187         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21188         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21189         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21190 
21191         // Force IllegalArgumentException
21192         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21193 
21194         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21195                 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz));
21196     }
21197 
21198     @Test
21199     void testConstructor183() throws WrongSizeException {
21200         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21201 
21202         final var ba = generateBa();
21203         final var biasX = ba.getElementAtIndex(0);
21204         final var biasY = ba.getElementAtIndex(1);
21205         final var biasZ = ba.getElementAtIndex(2);
21206 
21207         final var ma = generateMaCommonAxis();
21208         final var sx = ma.getElementAt(0, 0);
21209         final var sy = ma.getElementAt(1, 1);
21210         final var sz = ma.getElementAt(2, 2);
21211 
21212         final var randomizer = new UniformRandomizer();
21213         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21214         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21215         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21216         final var nedPosition = new NEDPosition(latitude, longitude, height);
21217         final var nedVelocity = new NEDVelocity();
21218         final var ecefPosition = new ECEFPosition();
21219         final var ecefVelocity = new ECEFVelocity();
21220         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21221         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21222                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21223         final var gravityNorm = gravity.getNormAsAcceleration();
21224 
21225         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21226                 biasX, biasY, biasZ, sx, sy, sz, this);
21227 
21228         // check default values
21229         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21230         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21231         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21232         final var bx1 = calibrator.getBiasXAsAcceleration();
21233         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21234         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21235         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21236         calibrator.getBiasXAsAcceleration(bx2);
21237         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21238         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21239         final var by1 = calibrator.getBiasYAsAcceleration();
21240         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21241         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21242         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21243         calibrator.getBiasYAsAcceleration(by2);
21244         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21245         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21246         final var bz1 = calibrator.getBiasZAsAcceleration();
21247         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21248         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21249         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21250         calibrator.getBiasZAsAcceleration(bz2);
21251         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21252         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21253         final var biasTriad1 = calibrator.getBiasAsTriad();
21254         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21255         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21256         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21257         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21258         final var biasTriad2 = new AccelerationTriad();
21259         calibrator.getBiasAsTriad(biasTriad2);
21260         assertEquals(biasTriad1, biasTriad2);
21261         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21262         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21263         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21264         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21265         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21266         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21267         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21268         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21269         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21270         final var bias1 = calibrator.getBias();
21271         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21272         final var bias2 = new double[3];
21273         calibrator.getBias(bias2);
21274         assertArrayEquals(bias1, bias2, 0.0);
21275         final var b1 = calibrator.getBiasAsMatrix();
21276         assertEquals(b1, ba);
21277         final var b2 = new Matrix(3, 1);
21278         calibrator.getBiasAsMatrix(b2);
21279         assertEquals(b1, b2);
21280         final var ma1 = calibrator.getInitialMa();
21281         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21282         final var ma2 = new Matrix(3, 3);
21283         calibrator.getInitialMa(ma2);
21284         assertEquals(ma1, ma2);
21285         assertSame(measurements, calibrator.getMeasurements());
21286         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21287                 calibrator.getMeasurementType());
21288         assertFalse(calibrator.isOrderedMeasurementsRequired());
21289         assertFalse(calibrator.isQualityScoresRequired());
21290         assertFalse(calibrator.isCommonAxisUsed());
21291         assertSame(this, calibrator.getListener());
21292         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21293         assertFalse(calibrator.isReady());
21294         assertFalse(calibrator.isRunning());
21295         assertNull(calibrator.getEstimatedMa());
21296         assertNull(calibrator.getEstimatedSx());
21297         assertNull(calibrator.getEstimatedSy());
21298         assertNull(calibrator.getEstimatedSz());
21299         assertNull(calibrator.getEstimatedMxy());
21300         assertNull(calibrator.getEstimatedMxz());
21301         assertNull(calibrator.getEstimatedMyx());
21302         assertNull(calibrator.getEstimatedMyz());
21303         assertNull(calibrator.getEstimatedMzx());
21304         assertNull(calibrator.getEstimatedMzy());
21305         assertNull(calibrator.getEstimatedCovariance());
21306         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21307         assertNotNull(calibrator.getGroundTruthGravityNorm());
21308         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21309         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21310         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21311                 ABSOLUTE_ERROR));
21312         final var g = new Acceleration(0.0, AccelerationUnit.G);
21313         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21314         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21315         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21316 
21317         // Force IllegalArgumentException
21318         final var invalidGravityNorm = new Acceleration(
21319                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21320 
21321         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21322                 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, this));
21323     }
21324 
21325     @Test
21326     void testConstructor184() throws WrongSizeException {
21327         final var ba = generateBa();
21328         final var biasX = ba.getElementAtIndex(0);
21329         final var biasY = ba.getElementAtIndex(1);
21330         final var biasZ = ba.getElementAtIndex(2);
21331 
21332         final var ma = generateMaCommonAxis();
21333         final var sx = ma.getElementAt(0, 0);
21334         final var sy = ma.getElementAt(1, 1);
21335         final var sz = ma.getElementAt(2, 2);
21336 
21337         final var randomizer = new UniformRandomizer();
21338         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21339         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21340         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21341         final var nedPosition = new NEDPosition(latitude, longitude, height);
21342         final var nedVelocity = new NEDVelocity();
21343         final var ecefPosition = new ECEFPosition();
21344         final var ecefVelocity = new ECEFVelocity();
21345         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21346         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21347                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21348         final var gravityNorm = gravity.getNormAsAcceleration();
21349 
21350         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
21351                 biasX, biasY, biasZ, sx, sy, sz);
21352 
21353         // check default values
21354         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21355         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21356         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21357         final var bx1 = calibrator.getBiasXAsAcceleration();
21358         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21359         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21360         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21361         calibrator.getBiasXAsAcceleration(bx2);
21362         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21363         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21364         final var by1 = calibrator.getBiasYAsAcceleration();
21365         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21366         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21367         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21368         calibrator.getBiasYAsAcceleration(by2);
21369         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21370         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21371         final var bz1 = calibrator.getBiasZAsAcceleration();
21372         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21373         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21374         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21375         calibrator.getBiasZAsAcceleration(bz2);
21376         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21377         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21378         final var biasTriad1 = calibrator.getBiasAsTriad();
21379         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21380         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21381         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21382         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21383         final var biasTriad2 = new AccelerationTriad();
21384         calibrator.getBiasAsTriad(biasTriad2);
21385         assertEquals(biasTriad1, biasTriad2);
21386         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21387         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21388         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21389         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21390         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21391         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21392         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21393         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21394         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21395         final var bias1 = calibrator.getBias();
21396         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21397         final var bias2 = new double[3];
21398         calibrator.getBias(bias2);
21399         assertArrayEquals(bias1, bias2, 0.0);
21400         final var b1 = calibrator.getBiasAsMatrix();
21401         assertEquals(b1, ba);
21402         final var b2 = new Matrix(3, 1);
21403         calibrator.getBiasAsMatrix(b2);
21404         assertEquals(b1, b2);
21405         final var ma1 = calibrator.getInitialMa();
21406         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21407         final var ma2 = new Matrix(3, 3);
21408         calibrator.getInitialMa(ma2);
21409         assertEquals(ma1, ma2);
21410         assertNull(calibrator.getMeasurements());
21411         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21412                 calibrator.getMeasurementType());
21413         assertFalse(calibrator.isOrderedMeasurementsRequired());
21414         assertFalse(calibrator.isQualityScoresRequired());
21415         assertTrue(calibrator.isCommonAxisUsed());
21416         assertNull(calibrator.getListener());
21417         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21418         assertFalse(calibrator.isReady());
21419         assertFalse(calibrator.isRunning());
21420         assertNull(calibrator.getEstimatedMa());
21421         assertNull(calibrator.getEstimatedSx());
21422         assertNull(calibrator.getEstimatedSy());
21423         assertNull(calibrator.getEstimatedSz());
21424         assertNull(calibrator.getEstimatedMxy());
21425         assertNull(calibrator.getEstimatedMxz());
21426         assertNull(calibrator.getEstimatedMyx());
21427         assertNull(calibrator.getEstimatedMyz());
21428         assertNull(calibrator.getEstimatedMzx());
21429         assertNull(calibrator.getEstimatedMzy());
21430         assertNull(calibrator.getEstimatedCovariance());
21431         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21432         assertNotNull(calibrator.getGroundTruthGravityNorm());
21433         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21434         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21435         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21436                 ABSOLUTE_ERROR));
21437         final var g = new Acceleration(0.0, AccelerationUnit.G);
21438         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21439         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21440         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21441 
21442         // Force IllegalArgumentException
21443         final var invalidGravityNorm = new Acceleration(
21444                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21445 
21446         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21447                 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz));
21448     }
21449 
21450     @Test
21451     void testConstructor185() throws WrongSizeException {
21452         final var ba = generateBa();
21453         final var biasX = ba.getElementAtIndex(0);
21454         final var biasY = ba.getElementAtIndex(1);
21455         final var biasZ = ba.getElementAtIndex(2);
21456 
21457         final var ma = generateMaCommonAxis();
21458         final var sx = ma.getElementAt(0, 0);
21459         final var sy = ma.getElementAt(1, 1);
21460         final var sz = ma.getElementAt(2, 2);
21461 
21462         final var randomizer = new UniformRandomizer();
21463         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21464         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21465         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21466         final var nedPosition = new NEDPosition(latitude, longitude, height);
21467         final var nedVelocity = new NEDVelocity();
21468         final var ecefPosition = new ECEFPosition();
21469         final var ecefVelocity = new ECEFVelocity();
21470         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21471         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21472                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21473         final var gravityNorm = gravity.getNormAsAcceleration();
21474 
21475         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
21476                 biasX, biasY, biasZ, sx, sy, sz, this);
21477 
21478         // check default values
21479         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21480         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21481         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21482         final var bx1 = calibrator.getBiasXAsAcceleration();
21483         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21484         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21485         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21486         calibrator.getBiasXAsAcceleration(bx2);
21487         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21488         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21489         final var by1 = calibrator.getBiasYAsAcceleration();
21490         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21491         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21492         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21493         calibrator.getBiasYAsAcceleration(by2);
21494         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21495         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21496         final var bz1 = calibrator.getBiasZAsAcceleration();
21497         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21498         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21499         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21500         calibrator.getBiasZAsAcceleration(bz2);
21501         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21502         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21503         final var biasTriad1 = calibrator.getBiasAsTriad();
21504         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21505         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21506         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21507         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21508         final var biasTriad2 = new AccelerationTriad();
21509         calibrator.getBiasAsTriad(biasTriad2);
21510         assertEquals(biasTriad1, biasTriad2);
21511         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21512         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21513         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21514         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21515         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21516         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21517         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21518         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21519         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21520         final var bias1 = calibrator.getBias();
21521         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21522         final var bias2 = new double[3];
21523         calibrator.getBias(bias2);
21524         assertArrayEquals(bias1, bias2, 0.0);
21525         final var b1 = calibrator.getBiasAsMatrix();
21526         assertEquals(b1, ba);
21527         final var b2 = new Matrix(3, 1);
21528         calibrator.getBiasAsMatrix(b2);
21529         assertEquals(b1, b2);
21530         final var ma1 = calibrator.getInitialMa();
21531         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21532         final var ma2 = new Matrix(3, 3);
21533         calibrator.getInitialMa(ma2);
21534         assertEquals(ma1, ma2);
21535         assertNull(calibrator.getMeasurements());
21536         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21537                 calibrator.getMeasurementType());
21538         assertFalse(calibrator.isOrderedMeasurementsRequired());
21539         assertFalse(calibrator.isQualityScoresRequired());
21540         assertTrue(calibrator.isCommonAxisUsed());
21541         assertSame(this, calibrator.getListener());
21542         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21543         assertFalse(calibrator.isReady());
21544         assertFalse(calibrator.isRunning());
21545         assertNull(calibrator.getEstimatedMa());
21546         assertNull(calibrator.getEstimatedSx());
21547         assertNull(calibrator.getEstimatedSy());
21548         assertNull(calibrator.getEstimatedSz());
21549         assertNull(calibrator.getEstimatedMxy());
21550         assertNull(calibrator.getEstimatedMxz());
21551         assertNull(calibrator.getEstimatedMyx());
21552         assertNull(calibrator.getEstimatedMyz());
21553         assertNull(calibrator.getEstimatedMzx());
21554         assertNull(calibrator.getEstimatedMzy());
21555         assertNull(calibrator.getEstimatedCovariance());
21556         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21557         assertNotNull(calibrator.getGroundTruthGravityNorm());
21558         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21559         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21560         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21561                 ABSOLUTE_ERROR));
21562         final var g = new Acceleration(0.0, AccelerationUnit.G);
21563         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21564         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21565         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21566 
21567         // Force IllegalArgumentException
21568         final var invalidGravityNorm = new Acceleration(
21569                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21570 
21571         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21572                 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, this));
21573     }
21574 
21575     @Test
21576     void testConstructor186() throws WrongSizeException {
21577         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21578 
21579         final var ba = generateBa();
21580         final var biasX = ba.getElementAtIndex(0);
21581         final var biasY = ba.getElementAtIndex(1);
21582         final var biasZ = ba.getElementAtIndex(2);
21583 
21584         final var ma = generateMaCommonAxis();
21585         final var sx = ma.getElementAt(0, 0);
21586         final var sy = ma.getElementAt(1, 1);
21587         final var sz = ma.getElementAt(2, 2);
21588 
21589         final var randomizer = new UniformRandomizer();
21590         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21591         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21592         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21593         final var nedPosition = new NEDPosition(latitude, longitude, height);
21594         final var nedVelocity = new NEDVelocity();
21595         final var ecefPosition = new ECEFPosition();
21596         final var ecefVelocity = new ECEFVelocity();
21597         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21598         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21599                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21600         final var gravityNorm = gravity.getNormAsAcceleration();
21601 
21602         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21603                 true, biasX, biasY, biasZ, sx, sy, sz);
21604 
21605         // check default values
21606         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21607         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21608         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21609         final var bx1 = calibrator.getBiasXAsAcceleration();
21610         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21611         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21612         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21613         calibrator.getBiasXAsAcceleration(bx2);
21614         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21615         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21616         final var by1 = calibrator.getBiasYAsAcceleration();
21617         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21618         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21619         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21620         calibrator.getBiasYAsAcceleration(by2);
21621         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21622         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21623         final var bz1 = calibrator.getBiasZAsAcceleration();
21624         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21625         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21626         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21627         calibrator.getBiasZAsAcceleration(bz2);
21628         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21629         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21630         final var biasTriad1 = calibrator.getBiasAsTriad();
21631         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21632         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21633         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21634         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21635         final var biasTriad2 = new AccelerationTriad();
21636         calibrator.getBiasAsTriad(biasTriad2);
21637         assertEquals(biasTriad1, biasTriad2);
21638         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21639         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21640         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21641         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21642         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21643         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21644         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21645         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21646         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21647         final var bias1 = calibrator.getBias();
21648         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21649         final var bias2 = new double[3];
21650         calibrator.getBias(bias2);
21651         assertArrayEquals(bias1, bias2, 0.0);
21652         final var b1 = calibrator.getBiasAsMatrix();
21653         assertEquals(b1, ba);
21654         final var b2 = new Matrix(3, 1);
21655         calibrator.getBiasAsMatrix(b2);
21656         assertEquals(b1, b2);
21657         final var ma1 = calibrator.getInitialMa();
21658         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21659         final var ma2 = new Matrix(3, 3);
21660         calibrator.getInitialMa(ma2);
21661         assertEquals(ma1, ma2);
21662         assertSame(measurements, calibrator.getMeasurements());
21663         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21664                 calibrator.getMeasurementType());
21665         assertFalse(calibrator.isOrderedMeasurementsRequired());
21666         assertFalse(calibrator.isQualityScoresRequired());
21667         assertTrue(calibrator.isCommonAxisUsed());
21668         assertNull(calibrator.getListener());
21669         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21670         assertFalse(calibrator.isReady());
21671         assertFalse(calibrator.isRunning());
21672         assertNull(calibrator.getEstimatedMa());
21673         assertNull(calibrator.getEstimatedSx());
21674         assertNull(calibrator.getEstimatedSy());
21675         assertNull(calibrator.getEstimatedSz());
21676         assertNull(calibrator.getEstimatedMxy());
21677         assertNull(calibrator.getEstimatedMxz());
21678         assertNull(calibrator.getEstimatedMyx());
21679         assertNull(calibrator.getEstimatedMyz());
21680         assertNull(calibrator.getEstimatedMzx());
21681         assertNull(calibrator.getEstimatedMzy());
21682         assertNull(calibrator.getEstimatedCovariance());
21683         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21684         assertNotNull(calibrator.getGroundTruthGravityNorm());
21685         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21686         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21687         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21688                 ABSOLUTE_ERROR));
21689         final var g = new Acceleration(0.0, AccelerationUnit.G);
21690         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21691         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21692         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21693 
21694         // Force IllegalArgumentException
21695         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21696 
21697         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21698                 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz));
21699     }
21700 
21701     @Test
21702     void testConstructor187() throws WrongSizeException {
21703         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
21704 
21705         final var ba = generateBa();
21706         final var biasX = ba.getElementAtIndex(0);
21707         final var biasY = ba.getElementAtIndex(1);
21708         final var biasZ = ba.getElementAtIndex(2);
21709 
21710         final var ma = generateMaCommonAxis();
21711         final var sx = ma.getElementAt(0, 0);
21712         final var sy = ma.getElementAt(1, 1);
21713         final var sz = ma.getElementAt(2, 2);
21714 
21715         final var randomizer = new UniformRandomizer();
21716         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21717         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21718         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21719         final var nedPosition = new NEDPosition(latitude, longitude, height);
21720         final var nedVelocity = new NEDVelocity();
21721         final var ecefPosition = new ECEFPosition();
21722         final var ecefVelocity = new ECEFVelocity();
21723         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21724         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21725                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21726         final var gravityNorm = gravity.getNormAsAcceleration();
21727 
21728         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
21729                 true, biasX, biasY, biasZ, sx, sy, sz, this);
21730 
21731         // check default values
21732         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21733         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21734         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21735         final var bx1 = calibrator.getBiasXAsAcceleration();
21736         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21737         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21738         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21739         calibrator.getBiasXAsAcceleration(bx2);
21740         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21741         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21742         final var by1 = calibrator.getBiasYAsAcceleration();
21743         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21744         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21745         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21746         calibrator.getBiasYAsAcceleration(by2);
21747         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21748         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21749         final var bz1 = calibrator.getBiasZAsAcceleration();
21750         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21751         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21752         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21753         calibrator.getBiasZAsAcceleration(bz2);
21754         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21755         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21756         final var biasTriad1 = calibrator.getBiasAsTriad();
21757         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21758         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21759         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21760         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21761         final var biasTriad2 = new AccelerationTriad();
21762         calibrator.getBiasAsTriad(biasTriad2);
21763         assertEquals(biasTriad1, biasTriad2);
21764         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21765         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21766         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21767         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21768         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21769         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21770         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21771         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21772         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21773         final var bias1 = calibrator.getBias();
21774         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21775         final var bias2 = new double[3];
21776         calibrator.getBias(bias2);
21777         assertArrayEquals(bias1, bias2, 0.0);
21778         final var b1 = calibrator.getBiasAsMatrix();
21779         assertEquals(b1, ba);
21780         final var b2 = new Matrix(3, 1);
21781         calibrator.getBiasAsMatrix(b2);
21782         assertEquals(b1, b2);
21783         final var ma1 = calibrator.getInitialMa();
21784         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21785         final var ma2 = new Matrix(3, 3);
21786         calibrator.getInitialMa(ma2);
21787         assertEquals(ma1, ma2);
21788         assertSame(measurements, calibrator.getMeasurements());
21789         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21790                 calibrator.getMeasurementType());
21791         assertFalse(calibrator.isOrderedMeasurementsRequired());
21792         assertFalse(calibrator.isQualityScoresRequired());
21793         assertTrue(calibrator.isCommonAxisUsed());
21794         assertSame(this, calibrator.getListener());
21795         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
21796         assertFalse(calibrator.isReady());
21797         assertFalse(calibrator.isRunning());
21798         assertNull(calibrator.getEstimatedMa());
21799         assertNull(calibrator.getEstimatedSx());
21800         assertNull(calibrator.getEstimatedSy());
21801         assertNull(calibrator.getEstimatedSz());
21802         assertNull(calibrator.getEstimatedMxy());
21803         assertNull(calibrator.getEstimatedMxz());
21804         assertNull(calibrator.getEstimatedMyx());
21805         assertNull(calibrator.getEstimatedMyz());
21806         assertNull(calibrator.getEstimatedMzx());
21807         assertNull(calibrator.getEstimatedMzy());
21808         assertNull(calibrator.getEstimatedCovariance());
21809         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21810         assertNotNull(calibrator.getGroundTruthGravityNorm());
21811         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21812         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21813         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21814                 ABSOLUTE_ERROR));
21815         final var g = new Acceleration(0.0, AccelerationUnit.G);
21816         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21817         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21818         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21819 
21820         // Force IllegalArgumentException
21821         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21822 
21823         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21824                 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, this));
21825     }
21826 
21827     @Test
21828     void testConstructor188() throws WrongSizeException {
21829         final var ba = generateBa();
21830         final var biasX = ba.getElementAtIndex(0);
21831         final var biasY = ba.getElementAtIndex(1);
21832         final var biasZ = ba.getElementAtIndex(2);
21833 
21834         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21835         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21836         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21837 
21838         final var ma = generateMaCommonAxis();
21839         final var sx = ma.getElementAt(0, 0);
21840         final var sy = ma.getElementAt(1, 1);
21841         final var sz = ma.getElementAt(2, 2);
21842 
21843         final var randomizer = new UniformRandomizer();
21844         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21845         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21846         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21847         final var nedPosition = new NEDPosition(latitude, longitude, height);
21848         final var nedVelocity = new NEDVelocity();
21849         final var ecefPosition = new ECEFPosition();
21850         final var ecefVelocity = new ECEFVelocity();
21851         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21852         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21853                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21854         final var gravityNorm = gravity.getNormAsAcceleration();
21855 
21856         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz);
21857 
21858         // check default values
21859         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21860         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21861         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21862         final var bx1 = calibrator.getBiasXAsAcceleration();
21863         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21864         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21865         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21866         calibrator.getBiasXAsAcceleration(bx2);
21867         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21868         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21869         final var by1 = calibrator.getBiasYAsAcceleration();
21870         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21871         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
21872         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21873         calibrator.getBiasYAsAcceleration(by2);
21874         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
21875         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
21876         final var bz1 = calibrator.getBiasZAsAcceleration();
21877         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
21878         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
21879         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21880         calibrator.getBiasZAsAcceleration(bz2);
21881         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
21882         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
21883         final var biasTriad1 = calibrator.getBiasAsTriad();
21884         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
21885         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
21886         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
21887         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
21888         final var biasTriad2 = new AccelerationTriad();
21889         calibrator.getBiasAsTriad(biasTriad2);
21890         assertEquals(biasTriad1, biasTriad2);
21891         assertEquals(sx, calibrator.getInitialSx(), 0.0);
21892         assertEquals(sy, calibrator.getInitialSy(), 0.0);
21893         assertEquals(sz, calibrator.getInitialSz(), 0.0);
21894         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
21895         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
21896         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
21897         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
21898         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
21899         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
21900         final var bias1 = calibrator.getBias();
21901         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21902         final var bias2 = new double[3];
21903         calibrator.getBias(bias2);
21904         assertArrayEquals(bias1, bias2, 0.0);
21905         final var b1 = calibrator.getBiasAsMatrix();
21906         assertEquals(b1, ba);
21907         final var b2 = new Matrix(3, 1);
21908         calibrator.getBiasAsMatrix(b2);
21909         assertEquals(b1, b2);
21910         final var ma1 = calibrator.getInitialMa();
21911         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
21912         final var ma2 = new Matrix(3, 3);
21913         calibrator.getInitialMa(ma2);
21914         assertEquals(ma1, ma2);
21915         assertNull(calibrator.getMeasurements());
21916         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
21917                 calibrator.getMeasurementType());
21918         assertFalse(calibrator.isOrderedMeasurementsRequired());
21919         assertFalse(calibrator.isQualityScoresRequired());
21920         assertFalse(calibrator.isCommonAxisUsed());
21921         assertNull(calibrator.getListener());
21922         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
21923         assertFalse(calibrator.isReady());
21924         assertFalse(calibrator.isRunning());
21925         assertNull(calibrator.getEstimatedMa());
21926         assertNull(calibrator.getEstimatedSx());
21927         assertNull(calibrator.getEstimatedSy());
21928         assertNull(calibrator.getEstimatedSz());
21929         assertNull(calibrator.getEstimatedMxy());
21930         assertNull(calibrator.getEstimatedMxz());
21931         assertNull(calibrator.getEstimatedMyx());
21932         assertNull(calibrator.getEstimatedMyz());
21933         assertNull(calibrator.getEstimatedMzx());
21934         assertNull(calibrator.getEstimatedMzy());
21935         assertNull(calibrator.getEstimatedCovariance());
21936         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
21937         assertNotNull(calibrator.getGroundTruthGravityNorm());
21938         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21939         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21940         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
21941                 ABSOLUTE_ERROR));
21942         final var g = new Acceleration(0.0, AccelerationUnit.G);
21943         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21944         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21945         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
21946 
21947         // Force IllegalArgumentException
21948         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21949 
21950         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
21951                 invalidGravityNorm, bx, by, bz, sx, sy, sz));
21952     }
21953 
21954     @Test
21955     void testConstructor189() throws WrongSizeException {
21956         final var ba = generateBa();
21957         final var biasX = ba.getElementAtIndex(0);
21958         final var biasY = ba.getElementAtIndex(1);
21959         final var biasZ = ba.getElementAtIndex(2);
21960 
21961         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21962         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21963         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
21964 
21965         final var ma = generateMaCommonAxis();
21966         final var sx = ma.getElementAt(0, 0);
21967         final var sy = ma.getElementAt(1, 1);
21968         final var sz = ma.getElementAt(2, 2);
21969 
21970         final var randomizer = new UniformRandomizer();
21971         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
21972         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21973         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21974         final var nedPosition = new NEDPosition(latitude, longitude, height);
21975         final var nedVelocity = new NEDVelocity();
21976         final var ecefPosition = new ECEFPosition();
21977         final var ecefVelocity = new ECEFVelocity();
21978         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
21979         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21980                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21981         final var gravityNorm = gravity.getNormAsAcceleration();
21982 
21983         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
21984                 this);
21985 
21986         // check default values
21987         assertEquals(biasX, calibrator.getBiasX(), 0.0);
21988         assertEquals(biasY, calibrator.getBiasY(), 0.0);
21989         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
21990         final var bx1 = calibrator.getBiasXAsAcceleration();
21991         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
21992         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
21993         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
21994         calibrator.getBiasXAsAcceleration(bx2);
21995         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
21996         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
21997         final var by1 = calibrator.getBiasYAsAcceleration();
21998         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
21999         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22000         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22001         calibrator.getBiasYAsAcceleration(by2);
22002         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22003         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22004         final var bz1 = calibrator.getBiasZAsAcceleration();
22005         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22006         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22007         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22008         calibrator.getBiasZAsAcceleration(bz2);
22009         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22010         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22011         final var biasTriad1 = calibrator.getBiasAsTriad();
22012         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22013         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22014         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22015         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22016         final var biasTriad2 = new AccelerationTriad();
22017         calibrator.getBiasAsTriad(biasTriad2);
22018         assertEquals(biasTriad1, biasTriad2);
22019         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22020         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22021         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22022         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22023         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22024         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22025         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22026         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22027         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22028         final var bias1 = calibrator.getBias();
22029         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22030         final var bias2 = new double[3];
22031         calibrator.getBias(bias2);
22032         assertArrayEquals(bias1, bias2, 0.0);
22033         final var b1 = calibrator.getBiasAsMatrix();
22034         assertEquals(b1, ba);
22035         final var b2 = new Matrix(3, 1);
22036         calibrator.getBiasAsMatrix(b2);
22037         assertEquals(b1, b2);
22038         final var ma1 = calibrator.getInitialMa();
22039         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22040         final var ma2 = new Matrix(3, 3);
22041         calibrator.getInitialMa(ma2);
22042         assertEquals(ma1, ma2);
22043         assertNull(calibrator.getMeasurements());
22044         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22045                 calibrator.getMeasurementType());
22046         assertFalse(calibrator.isOrderedMeasurementsRequired());
22047         assertFalse(calibrator.isQualityScoresRequired());
22048         assertFalse(calibrator.isCommonAxisUsed());
22049         assertSame(this, calibrator.getListener());
22050         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22051         assertFalse(calibrator.isReady());
22052         assertFalse(calibrator.isRunning());
22053         assertNull(calibrator.getEstimatedMa());
22054         assertNull(calibrator.getEstimatedSx());
22055         assertNull(calibrator.getEstimatedSy());
22056         assertNull(calibrator.getEstimatedSz());
22057         assertNull(calibrator.getEstimatedMxy());
22058         assertNull(calibrator.getEstimatedMxz());
22059         assertNull(calibrator.getEstimatedMyx());
22060         assertNull(calibrator.getEstimatedMyz());
22061         assertNull(calibrator.getEstimatedMzx());
22062         assertNull(calibrator.getEstimatedMzy());
22063         assertNull(calibrator.getEstimatedCovariance());
22064         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22065         assertNotNull(calibrator.getGroundTruthGravityNorm());
22066         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22067         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22068         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22069                 ABSOLUTE_ERROR));
22070         final var g = new Acceleration(0.0, AccelerationUnit.G);
22071         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22072         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22073         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22074 
22075         // Force IllegalArgumentException
22076         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22077 
22078         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22079                 invalidGravityNorm, bx, by, bz, sx, sy, sz, this));
22080     }
22081 
22082     @Test
22083     void testConstructor190() throws WrongSizeException {
22084         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22085 
22086         final var ba = generateBa();
22087         final var biasX = ba.getElementAtIndex(0);
22088         final var biasY = ba.getElementAtIndex(1);
22089         final var biasZ = ba.getElementAtIndex(2);
22090 
22091         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22092         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22093         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22094 
22095         final var ma = generateMaCommonAxis();
22096         final var sx = ma.getElementAt(0, 0);
22097         final var sy = ma.getElementAt(1, 1);
22098         final var sz = ma.getElementAt(2, 2);
22099 
22100         final var randomizer = new UniformRandomizer();
22101         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22102         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22103         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22104         final var nedPosition = new NEDPosition(latitude, longitude, height);
22105         final var nedVelocity = new NEDVelocity();
22106         final var ecefPosition = new ECEFPosition();
22107         final var ecefVelocity = new ECEFVelocity();
22108         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22109         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22110                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22111         final var gravityNorm = gravity.getNormAsAcceleration();
22112 
22113         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
22114                 sx, sy, sz);
22115 
22116         // check default values
22117         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22118         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22119         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22120         final var bx1 = calibrator.getBiasXAsAcceleration();
22121         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22122         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22123         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22124         calibrator.getBiasXAsAcceleration(bx2);
22125         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22126         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22127         final var by1 = calibrator.getBiasYAsAcceleration();
22128         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22129         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22130         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22131         calibrator.getBiasYAsAcceleration(by2);
22132         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22133         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22134         final var bz1 = calibrator.getBiasZAsAcceleration();
22135         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22136         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22137         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22138         calibrator.getBiasZAsAcceleration(bz2);
22139         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22140         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22141         final var biasTriad1 = calibrator.getBiasAsTriad();
22142         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22143         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22144         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22145         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22146         final var biasTriad2 = new AccelerationTriad();
22147         calibrator.getBiasAsTriad(biasTriad2);
22148         assertEquals(biasTriad1, biasTriad2);
22149         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22150         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22151         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22152         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22153         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22154         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22155         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22156         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22157         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22158         final var bias1 = calibrator.getBias();
22159         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22160         final var bias2 = new double[3];
22161         calibrator.getBias(bias2);
22162         assertArrayEquals(bias1, bias2, 0.0);
22163         final var b1 = calibrator.getBiasAsMatrix();
22164         assertEquals(b1, ba);
22165         final var b2 = new Matrix(3, 1);
22166         calibrator.getBiasAsMatrix(b2);
22167         assertEquals(b1, b2);
22168         final var ma1 = calibrator.getInitialMa();
22169         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22170         final var ma2 = new Matrix(3, 3);
22171         calibrator.getInitialMa(ma2);
22172         assertEquals(ma1, ma2);
22173         assertSame(measurements, calibrator.getMeasurements());
22174         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22175                 calibrator.getMeasurementType());
22176         assertFalse(calibrator.isOrderedMeasurementsRequired());
22177         assertFalse(calibrator.isQualityScoresRequired());
22178         assertFalse(calibrator.isCommonAxisUsed());
22179         assertNull(calibrator.getListener());
22180         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22181         assertFalse(calibrator.isReady());
22182         assertFalse(calibrator.isRunning());
22183         assertNull(calibrator.getEstimatedMa());
22184         assertNull(calibrator.getEstimatedSx());
22185         assertNull(calibrator.getEstimatedSy());
22186         assertNull(calibrator.getEstimatedSz());
22187         assertNull(calibrator.getEstimatedMxy());
22188         assertNull(calibrator.getEstimatedMxz());
22189         assertNull(calibrator.getEstimatedMyx());
22190         assertNull(calibrator.getEstimatedMyz());
22191         assertNull(calibrator.getEstimatedMzx());
22192         assertNull(calibrator.getEstimatedMzy());
22193         assertNull(calibrator.getEstimatedCovariance());
22194         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22195         assertNotNull(calibrator.getGroundTruthGravityNorm());
22196         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22197         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22198         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22199                 ABSOLUTE_ERROR));
22200         final var g = new Acceleration(0.0, AccelerationUnit.G);
22201         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22202         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22203         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22204 
22205         // Force IllegalArgumentException
22206         final var invalidGravityNorm = new Acceleration(
22207                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22208 
22209         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22210                 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz));
22211     }
22212 
22213     @Test
22214     void testConstructor191() throws WrongSizeException {
22215         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22216 
22217         final var ba = generateBa();
22218         final var biasX = ba.getElementAtIndex(0);
22219         final var biasY = ba.getElementAtIndex(1);
22220         final var biasZ = ba.getElementAtIndex(2);
22221 
22222         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22223         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22224         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22225 
22226         final var ma = generateMaCommonAxis();
22227         final var sx = ma.getElementAt(0, 0);
22228         final var sy = ma.getElementAt(1, 1);
22229         final var sz = ma.getElementAt(2, 2);
22230 
22231         final var randomizer = new UniformRandomizer();
22232         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22233         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22234         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22235         final var nedPosition = new NEDPosition(latitude, longitude, height);
22236         final var nedVelocity = new NEDVelocity();
22237         final var ecefPosition = new ECEFPosition();
22238         final var ecefVelocity = new ECEFVelocity();
22239         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22240         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22241                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22242         final var gravityNorm = gravity.getNormAsAcceleration();
22243 
22244         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
22245                 sx, sy, sz, this);
22246 
22247         // check default values
22248         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22249         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22250         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22251         final var bx1 = calibrator.getBiasXAsAcceleration();
22252         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22253         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22254         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22255         calibrator.getBiasXAsAcceleration(bx2);
22256         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22257         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22258         final var by1 = calibrator.getBiasYAsAcceleration();
22259         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22260         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22261         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22262         calibrator.getBiasYAsAcceleration(by2);
22263         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22264         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22265         final var bz1 = calibrator.getBiasZAsAcceleration();
22266         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22267         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22268         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22269         calibrator.getBiasZAsAcceleration(bz2);
22270         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22271         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22272         final var biasTriad1 = calibrator.getBiasAsTriad();
22273         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22274         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22275         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22276         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22277         final var biasTriad2 = new AccelerationTriad();
22278         calibrator.getBiasAsTriad(biasTriad2);
22279         assertEquals(biasTriad1, biasTriad2);
22280         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22281         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22282         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22283         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22284         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22285         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22286         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22287         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22288         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22289         final var bias1 = calibrator.getBias();
22290         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22291         final var bias2 = new double[3];
22292         calibrator.getBias(bias2);
22293         assertArrayEquals(bias1, bias2, 0.0);
22294         final var b1 = calibrator.getBiasAsMatrix();
22295         assertEquals(b1, ba);
22296         final var b2 = new Matrix(3, 1);
22297         calibrator.getBiasAsMatrix(b2);
22298         assertEquals(b1, b2);
22299         final var ma1 = calibrator.getInitialMa();
22300         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22301         final var ma2 = new Matrix(3, 3);
22302         calibrator.getInitialMa(ma2);
22303         assertEquals(ma1, ma2);
22304         assertSame(measurements, calibrator.getMeasurements());
22305         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22306                 calibrator.getMeasurementType());
22307         assertFalse(calibrator.isOrderedMeasurementsRequired());
22308         assertFalse(calibrator.isQualityScoresRequired());
22309         assertFalse(calibrator.isCommonAxisUsed());
22310         assertSame(this, calibrator.getListener());
22311         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22312         assertFalse(calibrator.isReady());
22313         assertFalse(calibrator.isRunning());
22314         assertNull(calibrator.getEstimatedMa());
22315         assertNull(calibrator.getEstimatedSx());
22316         assertNull(calibrator.getEstimatedSy());
22317         assertNull(calibrator.getEstimatedSz());
22318         assertNull(calibrator.getEstimatedMxy());
22319         assertNull(calibrator.getEstimatedMxz());
22320         assertNull(calibrator.getEstimatedMyx());
22321         assertNull(calibrator.getEstimatedMyz());
22322         assertNull(calibrator.getEstimatedMzx());
22323         assertNull(calibrator.getEstimatedMzy());
22324         assertNull(calibrator.getEstimatedCovariance());
22325         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22326         assertNotNull(calibrator.getGroundTruthGravityNorm());
22327         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22328         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22329         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22330                 ABSOLUTE_ERROR));
22331         final var g = new Acceleration(0.0, AccelerationUnit.G);
22332         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22333         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22334         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22335 
22336         // Force IllegalArgumentException
22337         final var invalidGravityNorm = new Acceleration(
22338                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22339 
22340         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22341                 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz, this));
22342     }
22343 
22344     @Test
22345     void testConstructor192() throws WrongSizeException {
22346         final var ba = generateBa();
22347         final var biasX = ba.getElementAtIndex(0);
22348         final var biasY = ba.getElementAtIndex(1);
22349         final var biasZ = ba.getElementAtIndex(2);
22350 
22351         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22352         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22353         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22354 
22355         final var ma = generateMaCommonAxis();
22356         final var sx = ma.getElementAt(0, 0);
22357         final var sy = ma.getElementAt(1, 1);
22358         final var sz = ma.getElementAt(2, 2);
22359 
22360         final var randomizer = new UniformRandomizer();
22361         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22362         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22363         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22364         final var nedPosition = new NEDPosition(latitude, longitude, height);
22365         final var nedVelocity = new NEDVelocity();
22366         final var ecefPosition = new ECEFPosition();
22367         final var ecefVelocity = new ECEFVelocity();
22368         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22369         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22370                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22371         final var gravityNorm = gravity.getNormAsAcceleration();
22372 
22373         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
22374                 bx, by, bz, sx, sy, sz);
22375 
22376         // check default values
22377         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22378         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22379         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22380         final var bx1 = calibrator.getBiasXAsAcceleration();
22381         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22382         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22383         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22384         calibrator.getBiasXAsAcceleration(bx2);
22385         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22386         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22387         final var by1 = calibrator.getBiasYAsAcceleration();
22388         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22389         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22390         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22391         calibrator.getBiasYAsAcceleration(by2);
22392         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22393         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22394         final var bz1 = calibrator.getBiasZAsAcceleration();
22395         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22396         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22397         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22398         calibrator.getBiasZAsAcceleration(bz2);
22399         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22400         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22401         final var biasTriad1 = calibrator.getBiasAsTriad();
22402         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22403         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22404         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22405         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22406         final var biasTriad2 = new AccelerationTriad();
22407         calibrator.getBiasAsTriad(biasTriad2);
22408         assertEquals(biasTriad1, biasTriad2);
22409         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22410         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22411         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22412         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22413         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22414         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22415         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22416         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22417         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22418         final var bias1 = calibrator.getBias();
22419         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22420         final var bias2 = new double[3];
22421         calibrator.getBias(bias2);
22422         assertArrayEquals(bias1, bias2, 0.0);
22423         final var b1 = calibrator.getBiasAsMatrix();
22424         assertEquals(b1, ba);
22425         final var b2 = new Matrix(3, 1);
22426         calibrator.getBiasAsMatrix(b2);
22427         assertEquals(b1, b2);
22428         final var ma1 = calibrator.getInitialMa();
22429         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22430         final var ma2 = new Matrix(3, 3);
22431         calibrator.getInitialMa(ma2);
22432         assertEquals(ma1, ma2);
22433         assertNull(calibrator.getMeasurements());
22434         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22435                 calibrator.getMeasurementType());
22436         assertFalse(calibrator.isOrderedMeasurementsRequired());
22437         assertFalse(calibrator.isQualityScoresRequired());
22438         assertTrue(calibrator.isCommonAxisUsed());
22439         assertNull(calibrator.getListener());
22440         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22441         assertFalse(calibrator.isReady());
22442         assertFalse(calibrator.isRunning());
22443         assertNull(calibrator.getEstimatedMa());
22444         assertNull(calibrator.getEstimatedSx());
22445         assertNull(calibrator.getEstimatedSy());
22446         assertNull(calibrator.getEstimatedSz());
22447         assertNull(calibrator.getEstimatedMxy());
22448         assertNull(calibrator.getEstimatedMxz());
22449         assertNull(calibrator.getEstimatedMyx());
22450         assertNull(calibrator.getEstimatedMyz());
22451         assertNull(calibrator.getEstimatedMzx());
22452         assertNull(calibrator.getEstimatedMzy());
22453         assertNull(calibrator.getEstimatedCovariance());
22454         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22455         assertNotNull(calibrator.getGroundTruthGravityNorm());
22456         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22457         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22458         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22459                 ABSOLUTE_ERROR));
22460         final var g = new Acceleration(0.0, AccelerationUnit.G);
22461         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22462         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22463         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22464 
22465         // Force IllegalArgumentException
22466         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22467 
22468         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22469                 invalidGravityNorm, true, bx, by, bz, sx, sy, sz));
22470     }
22471 
22472     @Test
22473     void testConstructor193() throws WrongSizeException {
22474         final var ba = generateBa();
22475         final var biasX = ba.getElementAtIndex(0);
22476         final var biasY = ba.getElementAtIndex(1);
22477         final var biasZ = ba.getElementAtIndex(2);
22478 
22479         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22480         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22481         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22482 
22483         final var ma = generateMaCommonAxis();
22484         final var sx = ma.getElementAt(0, 0);
22485         final var sy = ma.getElementAt(1, 1);
22486         final var sz = ma.getElementAt(2, 2);
22487 
22488         final var randomizer = new UniformRandomizer();
22489         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22490         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22491         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22492         final var nedPosition = new NEDPosition(latitude, longitude, height);
22493         final var nedVelocity = new NEDVelocity();
22494         final var ecefPosition = new ECEFPosition();
22495         final var ecefVelocity = new ECEFVelocity();
22496         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22497         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22498                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22499         final var gravityNorm = gravity.getNormAsAcceleration();
22500 
22501         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
22502                 bx, by, bz, sx, sy, sz, this);
22503 
22504         // check default values
22505         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22506         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22507         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22508         final var bx1 = calibrator.getBiasXAsAcceleration();
22509         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22510         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22511         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22512         calibrator.getBiasXAsAcceleration(bx2);
22513         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22514         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22515         final var by1 = calibrator.getBiasYAsAcceleration();
22516         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22517         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22518         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22519         calibrator.getBiasYAsAcceleration(by2);
22520         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22521         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22522         final var bz1 = calibrator.getBiasZAsAcceleration();
22523         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22524         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22525         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22526         calibrator.getBiasZAsAcceleration(bz2);
22527         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22528         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22529         final var biasTriad1 = calibrator.getBiasAsTriad();
22530         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22531         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22532         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22533         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22534         final var biasTriad2 = new AccelerationTriad();
22535         calibrator.getBiasAsTriad(biasTriad2);
22536         assertEquals(biasTriad1, biasTriad2);
22537         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22538         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22539         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22540         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22541         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22542         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22543         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22544         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22545         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22546         final var bias1 = calibrator.getBias();
22547         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22548         final var bias2 = new double[3];
22549         calibrator.getBias(bias2);
22550         assertArrayEquals(bias1, bias2, 0.0);
22551         final var b1 = calibrator.getBiasAsMatrix();
22552         assertEquals(b1, ba);
22553         final var b2 = new Matrix(3, 1);
22554         calibrator.getBiasAsMatrix(b2);
22555         assertEquals(b1, b2);
22556         final var ma1 = calibrator.getInitialMa();
22557         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22558         final var ma2 = new Matrix(3, 3);
22559         calibrator.getInitialMa(ma2);
22560         assertEquals(ma1, ma2);
22561         assertNull(calibrator.getMeasurements());
22562         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22563                 calibrator.getMeasurementType());
22564         assertFalse(calibrator.isOrderedMeasurementsRequired());
22565         assertFalse(calibrator.isQualityScoresRequired());
22566         assertTrue(calibrator.isCommonAxisUsed());
22567         assertSame(this, calibrator.getListener());
22568         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22569         assertFalse(calibrator.isReady());
22570         assertFalse(calibrator.isRunning());
22571         assertNull(calibrator.getEstimatedMa());
22572         assertNull(calibrator.getEstimatedSx());
22573         assertNull(calibrator.getEstimatedSy());
22574         assertNull(calibrator.getEstimatedSz());
22575         assertNull(calibrator.getEstimatedMxy());
22576         assertNull(calibrator.getEstimatedMxz());
22577         assertNull(calibrator.getEstimatedMyx());
22578         assertNull(calibrator.getEstimatedMyz());
22579         assertNull(calibrator.getEstimatedMzx());
22580         assertNull(calibrator.getEstimatedMzy());
22581         assertNull(calibrator.getEstimatedCovariance());
22582         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22583         assertNotNull(calibrator.getGroundTruthGravityNorm());
22584         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22585         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22586         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22587                 ABSOLUTE_ERROR));
22588         final var g = new Acceleration(0.0, AccelerationUnit.G);
22589         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22590         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22591         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22592 
22593         // Force IllegalArgumentException
22594         final var invalidGravityNorm = new Acceleration(
22595                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22596 
22597         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22598                 invalidGravityNorm, true, bx, by, bz, sx, sy, sz, this));
22599     }
22600 
22601     @Test
22602     void testConstructor194() throws WrongSizeException {
22603         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22604 
22605         final var ba = generateBa();
22606         final var biasX = ba.getElementAtIndex(0);
22607         final var biasY = ba.getElementAtIndex(1);
22608         final var biasZ = ba.getElementAtIndex(2);
22609 
22610         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22611         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22612         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22613 
22614         final var ma = generateMaCommonAxis();
22615         final var sx = ma.getElementAt(0, 0);
22616         final var sy = ma.getElementAt(1, 1);
22617         final var sz = ma.getElementAt(2, 2);
22618 
22619         final var randomizer = new UniformRandomizer();
22620         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22621         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22622         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22623         final var nedPosition = new NEDPosition(latitude, longitude, height);
22624         final var nedVelocity = new NEDVelocity();
22625         final var ecefPosition = new ECEFPosition();
22626         final var ecefVelocity = new ECEFVelocity();
22627         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22628         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22629                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22630         final var gravityNorm = gravity.getNormAsAcceleration();
22631 
22632         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
22633                 true, bx, by, bz, sx, sy, sz);
22634 
22635         // check default values
22636         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22637         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22638         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22639         final var bx1 = calibrator.getBiasXAsAcceleration();
22640         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22641         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22642         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22643         calibrator.getBiasXAsAcceleration(bx2);
22644         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22645         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22646         final var by1 = calibrator.getBiasYAsAcceleration();
22647         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22648         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22649         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22650         calibrator.getBiasYAsAcceleration(by2);
22651         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22652         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22653         final var bz1 = calibrator.getBiasZAsAcceleration();
22654         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22655         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22656         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22657         calibrator.getBiasZAsAcceleration(bz2);
22658         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22659         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22660         final var biasTriad1 = calibrator.getBiasAsTriad();
22661         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22662         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22663         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22664         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22665         final var biasTriad2 = new AccelerationTriad();
22666         calibrator.getBiasAsTriad(biasTriad2);
22667         assertEquals(biasTriad1, biasTriad2);
22668         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22669         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22670         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22671         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22672         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22673         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22674         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22675         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22676         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22677         final var bias1 = calibrator.getBias();
22678         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22679         final var bias2 = new double[3];
22680         calibrator.getBias(bias2);
22681         assertArrayEquals(bias1, bias2, 0.0);
22682         final var b1 = calibrator.getBiasAsMatrix();
22683         assertEquals(b1, ba);
22684         final var b2 = new Matrix(3, 1);
22685         calibrator.getBiasAsMatrix(b2);
22686         assertEquals(b1, b2);
22687         final var ma1 = calibrator.getInitialMa();
22688         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22689         final var ma2 = new Matrix(3, 3);
22690         calibrator.getInitialMa(ma2);
22691         assertEquals(ma1, ma2);
22692         assertSame(measurements, calibrator.getMeasurements());
22693         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22694                 calibrator.getMeasurementType());
22695         assertFalse(calibrator.isOrderedMeasurementsRequired());
22696         assertFalse(calibrator.isQualityScoresRequired());
22697         assertTrue(calibrator.isCommonAxisUsed());
22698         assertNull(calibrator.getListener());
22699         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22700         assertFalse(calibrator.isReady());
22701         assertFalse(calibrator.isRunning());
22702         assertNull(calibrator.getEstimatedMa());
22703         assertNull(calibrator.getEstimatedSx());
22704         assertNull(calibrator.getEstimatedSy());
22705         assertNull(calibrator.getEstimatedSz());
22706         assertNull(calibrator.getEstimatedMxy());
22707         assertNull(calibrator.getEstimatedMxz());
22708         assertNull(calibrator.getEstimatedMyx());
22709         assertNull(calibrator.getEstimatedMyz());
22710         assertNull(calibrator.getEstimatedMzx());
22711         assertNull(calibrator.getEstimatedMzy());
22712         assertNull(calibrator.getEstimatedCovariance());
22713         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22714         assertNotNull(calibrator.getGroundTruthGravityNorm());
22715         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22716         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22717         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22718                 ABSOLUTE_ERROR));
22719         final var g = new Acceleration(0.0, AccelerationUnit.G);
22720         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22721         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22722         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22723 
22724         // Force IllegalArgumentException
22725         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22726 
22727         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22728                 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz));
22729     }
22730 
22731     @Test
22732     void testConstructor195() throws WrongSizeException {
22733         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22734 
22735         final var ba = generateBa();
22736         final var biasX = ba.getElementAtIndex(0);
22737         final var biasY = ba.getElementAtIndex(1);
22738         final var biasZ = ba.getElementAtIndex(2);
22739 
22740         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22741         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22742         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
22743 
22744         final var ma = generateMaCommonAxis();
22745         final var sx = ma.getElementAt(0, 0);
22746         final var sy = ma.getElementAt(1, 1);
22747         final var sz = ma.getElementAt(2, 2);
22748 
22749         final var randomizer = new UniformRandomizer();
22750         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22751         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22752         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22753         final var nedPosition = new NEDPosition(latitude, longitude, height);
22754         final var nedVelocity = new NEDVelocity();
22755         final var ecefPosition = new ECEFPosition();
22756         final var ecefVelocity = new ECEFVelocity();
22757         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22758         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22759                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22760         final var gravityNorm = gravity.getNormAsAcceleration();
22761 
22762         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
22763                 true, bx, by, bz, sx, sy, sz, this);
22764 
22765         // check default values
22766         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22767         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22768         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22769         final var bx1 = calibrator.getBiasXAsAcceleration();
22770         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22771         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22772         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22773         calibrator.getBiasXAsAcceleration(bx2);
22774         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22775         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22776         final var by1 = calibrator.getBiasYAsAcceleration();
22777         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22778         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22779         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22780         calibrator.getBiasYAsAcceleration(by2);
22781         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22782         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22783         final var bz1 = calibrator.getBiasZAsAcceleration();
22784         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22785         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22786         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22787         calibrator.getBiasZAsAcceleration(bz2);
22788         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22789         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22790         final var biasTriad1 = calibrator.getBiasAsTriad();
22791         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22792         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22793         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22794         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22795         final var biasTriad2 = new AccelerationTriad();
22796         calibrator.getBiasAsTriad(biasTriad2);
22797         assertEquals(biasTriad1, biasTriad2);
22798         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22799         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22800         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22801         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
22802         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
22803         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
22804         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
22805         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
22806         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
22807         final var bias1 = calibrator.getBias();
22808         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22809         final var bias2 = new double[3];
22810         calibrator.getBias(bias2);
22811         assertArrayEquals(bias1, bias2, 0.0);
22812         final var b1 = calibrator.getBiasAsMatrix();
22813         assertEquals(b1, ba);
22814         final var b2 = new Matrix(3, 1);
22815         calibrator.getBiasAsMatrix(b2);
22816         assertEquals(b1, b2);
22817         final var ma1 = calibrator.getInitialMa();
22818         assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
22819         final var ma2 = new Matrix(3, 3);
22820         calibrator.getInitialMa(ma2);
22821         assertEquals(ma1, ma2);
22822         assertSame(measurements, calibrator.getMeasurements());
22823         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22824                 calibrator.getMeasurementType());
22825         assertFalse(calibrator.isOrderedMeasurementsRequired());
22826         assertFalse(calibrator.isQualityScoresRequired());
22827         assertTrue(calibrator.isCommonAxisUsed());
22828         assertSame(this, calibrator.getListener());
22829         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
22830         assertFalse(calibrator.isReady());
22831         assertFalse(calibrator.isRunning());
22832         assertNull(calibrator.getEstimatedMa());
22833         assertNull(calibrator.getEstimatedSx());
22834         assertNull(calibrator.getEstimatedSy());
22835         assertNull(calibrator.getEstimatedSz());
22836         assertNull(calibrator.getEstimatedMxy());
22837         assertNull(calibrator.getEstimatedMxz());
22838         assertNull(calibrator.getEstimatedMyx());
22839         assertNull(calibrator.getEstimatedMyz());
22840         assertNull(calibrator.getEstimatedMzx());
22841         assertNull(calibrator.getEstimatedMzy());
22842         assertNull(calibrator.getEstimatedCovariance());
22843         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22844         assertNotNull(calibrator.getGroundTruthGravityNorm());
22845         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22846         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22847         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22848                 ABSOLUTE_ERROR));
22849         final var g = new Acceleration(0.0, AccelerationUnit.G);
22850         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22851         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22852         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22853 
22854         // Force IllegalArgumentException
22855         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22856 
22857         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22858                 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz, this));
22859     }
22860 
22861     @Test
22862     void testConstructor196() throws WrongSizeException {
22863         final var ba = generateBa();
22864         final var biasX = ba.getElementAtIndex(0);
22865         final var biasY = ba.getElementAtIndex(1);
22866         final var biasZ = ba.getElementAtIndex(2);
22867 
22868         final var ma = generateMaCommonAxis();
22869         final var sx = ma.getElementAt(0, 0);
22870         final var sy = ma.getElementAt(1, 1);
22871         final var sz = ma.getElementAt(2, 2);
22872         final var mxy = ma.getElementAt(0, 1);
22873         final var mxz = ma.getElementAt(0, 2);
22874         final var myx = ma.getElementAt(1, 0);
22875         final var myz = ma.getElementAt(1, 2);
22876         final var mzx = ma.getElementAt(2, 0);
22877         final var mzy = ma.getElementAt(2, 1);
22878 
22879         final var randomizer = new UniformRandomizer();
22880         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
22881         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22882         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22883         final var nedPosition = new NEDPosition(latitude, longitude, height);
22884         final var nedVelocity = new NEDVelocity();
22885         final var ecefPosition = new ECEFPosition();
22886         final var ecefVelocity = new ECEFVelocity();
22887         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
22888         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22889                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22890         final var gravityNorm = gravity.getNormAsAcceleration();
22891 
22892         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, biasX, biasY, biasZ,
22893                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
22894 
22895         // check default values
22896         assertEquals(biasX, calibrator.getBiasX(), 0.0);
22897         assertEquals(biasY, calibrator.getBiasY(), 0.0);
22898         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
22899         final var bx1 = calibrator.getBiasXAsAcceleration();
22900         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
22901         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
22902         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22903         calibrator.getBiasXAsAcceleration(bx2);
22904         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
22905         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
22906         final var by1 = calibrator.getBiasYAsAcceleration();
22907         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
22908         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
22909         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22910         calibrator.getBiasYAsAcceleration(by2);
22911         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
22912         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
22913         final var bz1 = calibrator.getBiasZAsAcceleration();
22914         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
22915         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
22916         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
22917         calibrator.getBiasZAsAcceleration(bz2);
22918         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
22919         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
22920         final var biasTriad1 = calibrator.getBiasAsTriad();
22921         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
22922         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
22923         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
22924         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
22925         final var biasTriad2 = new AccelerationTriad();
22926         calibrator.getBiasAsTriad(biasTriad2);
22927         assertEquals(biasTriad1, biasTriad2);
22928         assertEquals(sx, calibrator.getInitialSx(), 0.0);
22929         assertEquals(sy, calibrator.getInitialSy(), 0.0);
22930         assertEquals(sz, calibrator.getInitialSz(), 0.0);
22931         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
22932         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
22933         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
22934         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
22935         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
22936         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
22937         final var bias1 = calibrator.getBias();
22938         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22939         final var bias2 = new double[3];
22940         calibrator.getBias(bias2);
22941         assertArrayEquals(bias1, bias2, 0.0);
22942         final var b1 = calibrator.getBiasAsMatrix();
22943         assertEquals(b1, ba);
22944         final var b2 = new Matrix(3, 1);
22945         calibrator.getBiasAsMatrix(b2);
22946         assertEquals(b1, b2);
22947         final var ma1 = new Matrix(3, 3);
22948         ma1.setSubmatrix(0, 0, 2, 2,
22949                 new double[]{sx, myx, mzx,
22950                         mxy, sy, mzy,
22951                         mxz, myz, sz});
22952         assertEquals(calibrator.getInitialMa(), ma1);
22953         final var ma2 = new Matrix(3, 3);
22954         calibrator.getInitialMa(ma2);
22955         assertEquals(ma1, ma2);
22956         assertNull(calibrator.getMeasurements());
22957         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
22958                 calibrator.getMeasurementType());
22959         assertFalse(calibrator.isOrderedMeasurementsRequired());
22960         assertFalse(calibrator.isQualityScoresRequired());
22961         assertFalse(calibrator.isCommonAxisUsed());
22962         assertNull(calibrator.getListener());
22963         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
22964         assertFalse(calibrator.isReady());
22965         assertFalse(calibrator.isRunning());
22966         assertNull(calibrator.getEstimatedMa());
22967         assertNull(calibrator.getEstimatedSx());
22968         assertNull(calibrator.getEstimatedSy());
22969         assertNull(calibrator.getEstimatedSz());
22970         assertNull(calibrator.getEstimatedMxy());
22971         assertNull(calibrator.getEstimatedMxz());
22972         assertNull(calibrator.getEstimatedMyx());
22973         assertNull(calibrator.getEstimatedMyz());
22974         assertNull(calibrator.getEstimatedMzx());
22975         assertNull(calibrator.getEstimatedMzy());
22976         assertNull(calibrator.getEstimatedCovariance());
22977         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
22978         assertNotNull(calibrator.getGroundTruthGravityNorm());
22979         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22980         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22981         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
22982                 ABSOLUTE_ERROR));
22983         final var g = new Acceleration(0.0, AccelerationUnit.G);
22984         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22985         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22986         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
22987 
22988         // Force IllegalArgumentException
22989         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22990 
22991         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
22992                 invalidGravityNorm, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
22993     }
22994 
22995     @Test
22996     void testConstructor197() throws WrongSizeException {
22997         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
22998 
22999         final var ba = generateBa();
23000         final var biasX = ba.getElementAtIndex(0);
23001         final var biasY = ba.getElementAtIndex(1);
23002         final var biasZ = ba.getElementAtIndex(2);
23003 
23004         final var ma = generateMaCommonAxis();
23005         final var sx = ma.getElementAt(0, 0);
23006         final var sy = ma.getElementAt(1, 1);
23007         final var sz = ma.getElementAt(2, 2);
23008         final var mxy = ma.getElementAt(0, 1);
23009         final var mxz = ma.getElementAt(0, 2);
23010         final var myx = ma.getElementAt(1, 0);
23011         final var myz = ma.getElementAt(1, 2);
23012         final var mzx = ma.getElementAt(2, 0);
23013         final var mzy = ma.getElementAt(2, 1);
23014 
23015         final var randomizer = new UniformRandomizer();
23016         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23017         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23018         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23019         final var nedPosition = new NEDPosition(latitude, longitude, height);
23020         final var nedVelocity = new NEDVelocity();
23021         final var ecefPosition = new ECEFPosition();
23022         final var ecefVelocity = new ECEFVelocity();
23023         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23024         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23025                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23026         final var gravityNorm = gravity.getNormAsAcceleration();
23027 
23028         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23029                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23030 
23031         // check default values
23032         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23033         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23034         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23035         final var bx1 = calibrator.getBiasXAsAcceleration();
23036         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23037         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23038         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23039         calibrator.getBiasXAsAcceleration(bx2);
23040         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23041         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23042         final var by1 = calibrator.getBiasYAsAcceleration();
23043         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23044         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23045         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23046         calibrator.getBiasYAsAcceleration(by2);
23047         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23048         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23049         final var bz1 = calibrator.getBiasZAsAcceleration();
23050         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23051         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23052         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23053         calibrator.getBiasZAsAcceleration(bz2);
23054         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23055         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23056         final var biasTriad1 = calibrator.getBiasAsTriad();
23057         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23058         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23059         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23060         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23061         final var biasTriad2 = new AccelerationTriad();
23062         calibrator.getBiasAsTriad(biasTriad2);
23063         assertEquals(biasTriad1, biasTriad2);
23064         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23065         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23066         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23067         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23068         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23069         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23070         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23071         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23072         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23073         final var bias1 = calibrator.getBias();
23074         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23075         final var bias2 = new double[3];
23076         calibrator.getBias(bias2);
23077         assertArrayEquals(bias1, bias2, 0.0);
23078         final var b1 = calibrator.getBiasAsMatrix();
23079         assertEquals(b1, ba);
23080         final var b2 = new Matrix(3, 1);
23081         calibrator.getBiasAsMatrix(b2);
23082         assertEquals(b1, b2);
23083         final var ma1 = new Matrix(3, 3);
23084         ma1.setSubmatrix(0, 0, 2, 2,
23085                 new double[]{sx, myx, mzx,
23086                         mxy, sy, mzy,
23087                         mxz, myz, sz});
23088         assertEquals(calibrator.getInitialMa(), ma1);
23089         final var ma2 = new Matrix(3, 3);
23090         calibrator.getInitialMa(ma2);
23091         assertEquals(ma1, ma2);
23092         assertSame(measurements, calibrator.getMeasurements());
23093         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23094                 calibrator.getMeasurementType());
23095         assertFalse(calibrator.isOrderedMeasurementsRequired());
23096         assertFalse(calibrator.isQualityScoresRequired());
23097         assertFalse(calibrator.isCommonAxisUsed());
23098         assertNull(calibrator.getListener());
23099         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
23100         assertFalse(calibrator.isReady());
23101         assertFalse(calibrator.isRunning());
23102         assertNull(calibrator.getEstimatedMa());
23103         assertNull(calibrator.getEstimatedSx());
23104         assertNull(calibrator.getEstimatedSy());
23105         assertNull(calibrator.getEstimatedSz());
23106         assertNull(calibrator.getEstimatedMxy());
23107         assertNull(calibrator.getEstimatedMxz());
23108         assertNull(calibrator.getEstimatedMyx());
23109         assertNull(calibrator.getEstimatedMyz());
23110         assertNull(calibrator.getEstimatedMzx());
23111         assertNull(calibrator.getEstimatedMzy());
23112         assertNull(calibrator.getEstimatedCovariance());
23113         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23114         assertNotNull(calibrator.getGroundTruthGravityNorm());
23115         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23116         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23117         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23118                 ABSOLUTE_ERROR));
23119         final var g = new Acceleration(0.0, AccelerationUnit.G);
23120         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23121         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23122         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23123 
23124         // Force IllegalArgumentException
23125         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23126 
23127         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23128                 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23129     }
23130 
23131     @Test
23132     void testConstructor198() throws WrongSizeException {
23133         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
23134 
23135         final var ba = generateBa();
23136         final var biasX = ba.getElementAtIndex(0);
23137         final var biasY = ba.getElementAtIndex(1);
23138         final var biasZ = ba.getElementAtIndex(2);
23139 
23140         final var ma = generateMaCommonAxis();
23141         final var sx = ma.getElementAt(0, 0);
23142         final var sy = ma.getElementAt(1, 1);
23143         final var sz = ma.getElementAt(2, 2);
23144         final var mxy = ma.getElementAt(0, 1);
23145         final var mxz = ma.getElementAt(0, 2);
23146         final var myx = ma.getElementAt(1, 0);
23147         final var myz = ma.getElementAt(1, 2);
23148         final var mzx = ma.getElementAt(2, 0);
23149         final var mzy = ma.getElementAt(2, 1);
23150 
23151         final var randomizer = new UniformRandomizer();
23152         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23153         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23154         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23155         final var nedPosition = new NEDPosition(latitude, longitude, height);
23156         final var nedVelocity = new NEDVelocity();
23157         final var ecefPosition = new ECEFPosition();
23158         final var ecefVelocity = new ECEFVelocity();
23159         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23160         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23161                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23162         final var gravityNorm = gravity.getNormAsAcceleration();
23163 
23164         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23165                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
23166 
23167         // check default values
23168         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23169         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23170         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23171         final var bx1 = calibrator.getBiasXAsAcceleration();
23172         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23173         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23174         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23175         calibrator.getBiasXAsAcceleration(bx2);
23176         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23177         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23178         final var by1 = calibrator.getBiasYAsAcceleration();
23179         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23180         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23181         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23182         calibrator.getBiasYAsAcceleration(by2);
23183         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23184         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23185         final var bz1 = calibrator.getBiasZAsAcceleration();
23186         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23187         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23188         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23189         calibrator.getBiasZAsAcceleration(bz2);
23190         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23191         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23192         final var biasTriad1 = calibrator.getBiasAsTriad();
23193         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23194         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23195         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23196         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23197         final var biasTriad2 = new AccelerationTriad();
23198         calibrator.getBiasAsTriad(biasTriad2);
23199         assertEquals(biasTriad1, biasTriad2);
23200         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23201         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23202         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23203         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23204         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23205         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23206         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23207         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23208         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23209         final var bias1 = calibrator.getBias();
23210         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23211         final var bias2 = new double[3];
23212         calibrator.getBias(bias2);
23213         assertArrayEquals(bias1, bias2, 0.0);
23214         final var b1 = calibrator.getBiasAsMatrix();
23215         assertEquals(b1, ba);
23216         final var b2 = new Matrix(3, 1);
23217         calibrator.getBiasAsMatrix(b2);
23218         assertEquals(b1, b2);
23219         final var ma1 = new Matrix(3, 3);
23220         ma1.setSubmatrix(0, 0, 2, 2,
23221                 new double[]{sx, myx, mzx,
23222                         mxy, sy, mzy,
23223                         mxz, myz, sz});
23224         assertEquals(ma1, calibrator.getInitialMa());
23225         final var ma2 = new Matrix(3, 3);
23226         calibrator.getInitialMa(ma2);
23227         assertEquals(ma1, ma2);
23228         assertSame(measurements, calibrator.getMeasurements());
23229         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23230                 calibrator.getMeasurementType());
23231         assertFalse(calibrator.isOrderedMeasurementsRequired());
23232         assertFalse(calibrator.isQualityScoresRequired());
23233         assertFalse(calibrator.isCommonAxisUsed());
23234         assertSame(this, calibrator.getListener());
23235         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
23236         assertFalse(calibrator.isReady());
23237         assertFalse(calibrator.isRunning());
23238         assertNull(calibrator.getEstimatedMa());
23239         assertNull(calibrator.getEstimatedSx());
23240         assertNull(calibrator.getEstimatedSy());
23241         assertNull(calibrator.getEstimatedSz());
23242         assertNull(calibrator.getEstimatedMxy());
23243         assertNull(calibrator.getEstimatedMxz());
23244         assertNull(calibrator.getEstimatedMyx());
23245         assertNull(calibrator.getEstimatedMyz());
23246         assertNull(calibrator.getEstimatedMzx());
23247         assertNull(calibrator.getEstimatedMzy());
23248         assertNull(calibrator.getEstimatedCovariance());
23249         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23250         assertNotNull(calibrator.getGroundTruthGravityNorm());
23251         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23252         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23253         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23254                 ABSOLUTE_ERROR));
23255         final var g = new Acceleration(0.0, AccelerationUnit.G);
23256         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23257         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23258         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23259 
23260         // Force IllegalArgumentException
23261         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23262 
23263         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23264                 invalidGravityNorm, measurements, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
23265                 this));
23266     }
23267 
23268     @Test
23269     void testConstructor199() throws WrongSizeException {
23270         final var ba = generateBa();
23271         final var biasX = ba.getElementAtIndex(0);
23272         final var biasY = ba.getElementAtIndex(1);
23273         final var biasZ = ba.getElementAtIndex(2);
23274 
23275         final var ma = generateMaCommonAxis();
23276         final var sx = ma.getElementAt(0, 0);
23277         final var sy = ma.getElementAt(1, 1);
23278         final var sz = ma.getElementAt(2, 2);
23279         final var mxy = ma.getElementAt(0, 1);
23280         final var mxz = ma.getElementAt(0, 2);
23281         final var myx = ma.getElementAt(1, 0);
23282         final var myz = ma.getElementAt(1, 2);
23283         final var mzx = ma.getElementAt(2, 0);
23284         final var mzy = ma.getElementAt(2, 1);
23285 
23286         final var randomizer = new UniformRandomizer();
23287         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23288         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23289         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23290         final var nedPosition = new NEDPosition(latitude, longitude, height);
23291         final var nedVelocity = new NEDVelocity();
23292         final var ecefPosition = new ECEFPosition();
23293         final var ecefVelocity = new ECEFVelocity();
23294         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23295         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23296                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23297         final var gravityNorm = gravity.getNormAsAcceleration();
23298 
23299         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
23300                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23301 
23302         // check default values
23303         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23304         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23305         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23306         final var bx1 = calibrator.getBiasXAsAcceleration();
23307         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23308         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23309         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23310         calibrator.getBiasXAsAcceleration(bx2);
23311         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23312         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23313         final var by1 = calibrator.getBiasYAsAcceleration();
23314         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23315         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23316         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23317         calibrator.getBiasYAsAcceleration(by2);
23318         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23319         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23320         final var bz1 = calibrator.getBiasZAsAcceleration();
23321         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23322         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23323         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23324         calibrator.getBiasZAsAcceleration(bz2);
23325         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23326         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23327         final var biasTriad1 = calibrator.getBiasAsTriad();
23328         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23329         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23330         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23331         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23332         final var biasTriad2 = new AccelerationTriad();
23333         calibrator.getBiasAsTriad(biasTriad2);
23334         assertEquals(biasTriad1, biasTriad2);
23335         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23336         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23337         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23338         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23339         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23340         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23341         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23342         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23343         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23344         final var bias1 = calibrator.getBias();
23345         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23346         final var bias2 = new double[3];
23347         calibrator.getBias(bias2);
23348         assertArrayEquals(bias1, bias2, 0.0);
23349         final var b1 = calibrator.getBiasAsMatrix();
23350         assertEquals(b1, ba);
23351         final var b2 = new Matrix(3, 1);
23352         calibrator.getBiasAsMatrix(b2);
23353         assertEquals(b1, b2);
23354         final var ma1 = new Matrix(3, 3);
23355         ma1.setSubmatrix(0, 0, 2, 2,
23356                 new double[]{sx, myx, mzx,
23357                         mxy, sy, mzy,
23358                         mxz, myz, sz});
23359         assertEquals(calibrator.getInitialMa(), ma1);
23360         final var ma2 = new Matrix(3, 3);
23361         calibrator.getInitialMa(ma2);
23362         assertEquals(ma1, ma2);
23363         assertNull(calibrator.getMeasurements());
23364         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23365                 calibrator.getMeasurementType());
23366         assertFalse(calibrator.isOrderedMeasurementsRequired());
23367         assertFalse(calibrator.isQualityScoresRequired());
23368         assertTrue(calibrator.isCommonAxisUsed());
23369         assertNull(calibrator.getListener());
23370         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23371         assertFalse(calibrator.isReady());
23372         assertFalse(calibrator.isRunning());
23373         assertNull(calibrator.getEstimatedMa());
23374         assertNull(calibrator.getEstimatedSx());
23375         assertNull(calibrator.getEstimatedSy());
23376         assertNull(calibrator.getEstimatedSz());
23377         assertNull(calibrator.getEstimatedMxy());
23378         assertNull(calibrator.getEstimatedMxz());
23379         assertNull(calibrator.getEstimatedMyx());
23380         assertNull(calibrator.getEstimatedMyz());
23381         assertNull(calibrator.getEstimatedMzx());
23382         assertNull(calibrator.getEstimatedMzy());
23383         assertNull(calibrator.getEstimatedCovariance());
23384         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23385         assertNotNull(calibrator.getGroundTruthGravityNorm());
23386         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23387         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23388         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23389                 ABSOLUTE_ERROR));
23390         final var g = new Acceleration(0.0, AccelerationUnit.G);
23391         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23392         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23393         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23394 
23395         // Force IllegalArgumentException
23396         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23397 
23398         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23399                 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23400     }
23401 
23402     @Test
23403     void testConstructor200() throws WrongSizeException {
23404         final var ba = generateBa();
23405         final var biasX = ba.getElementAtIndex(0);
23406         final var biasY = ba.getElementAtIndex(1);
23407         final var biasZ = ba.getElementAtIndex(2);
23408 
23409         final var ma = generateMaCommonAxis();
23410         final var sx = ma.getElementAt(0, 0);
23411         final var sy = ma.getElementAt(1, 1);
23412         final var sz = ma.getElementAt(2, 2);
23413         final var mxy = ma.getElementAt(0, 1);
23414         final var mxz = ma.getElementAt(0, 2);
23415         final var myx = ma.getElementAt(1, 0);
23416         final var myz = ma.getElementAt(1, 2);
23417         final var mzx = ma.getElementAt(2, 0);
23418         final var mzy = ma.getElementAt(2, 1);
23419 
23420         final var randomizer = new UniformRandomizer();
23421         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23422         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23423         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23424         final var nedPosition = new NEDPosition(latitude, longitude, height);
23425         final var nedVelocity = new NEDVelocity();
23426         final var ecefPosition = new ECEFPosition();
23427         final var ecefVelocity = new ECEFVelocity();
23428         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23429         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23430                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23431         final var gravityNorm = gravity.getNormAsAcceleration();
23432 
23433         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
23434                 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
23435 
23436         // check default values
23437         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23438         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23439         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23440         final var bx1 = calibrator.getBiasXAsAcceleration();
23441         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23442         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23443         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23444         calibrator.getBiasXAsAcceleration(bx2);
23445         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23446         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23447         final var by1 = calibrator.getBiasYAsAcceleration();
23448         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23449         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23450         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23451         calibrator.getBiasYAsAcceleration(by2);
23452         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23453         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23454         final var bz1 = calibrator.getBiasZAsAcceleration();
23455         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23456         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23457         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23458         calibrator.getBiasZAsAcceleration(bz2);
23459         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23460         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23461         final var biasTriad1 = calibrator.getBiasAsTriad();
23462         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23463         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23464         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23465         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23466         final var biasTriad2 = new AccelerationTriad();
23467         calibrator.getBiasAsTriad(biasTriad2);
23468         assertEquals(biasTriad1, biasTriad2);
23469         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23470         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23471         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23472         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23473         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23474         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23475         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23476         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23477         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23478         final var bias1 = calibrator.getBias();
23479         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23480         final var bias2 = new double[3];
23481         calibrator.getBias(bias2);
23482         assertArrayEquals(bias1, bias2, 0.0);
23483         final var b1 = calibrator.getBiasAsMatrix();
23484         assertEquals(b1, ba);
23485         final var b2 = new Matrix(3, 1);
23486         calibrator.getBiasAsMatrix(b2);
23487         assertEquals(b1, b2);
23488         final var ma1 = new Matrix(3, 3);
23489         ma1.setSubmatrix(0, 0, 2, 2,
23490                 new double[]{sx, myx, mzx,
23491                         mxy, sy, mzy,
23492                         mxz, myz, sz});
23493         assertEquals(ma1, calibrator.getInitialMa());
23494         final var ma2 = new Matrix(3, 3);
23495         calibrator.getInitialMa(ma2);
23496         assertEquals(ma1, ma2);
23497         assertNull(calibrator.getMeasurements());
23498         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23499                 calibrator.getMeasurementType());
23500         assertFalse(calibrator.isOrderedMeasurementsRequired());
23501         assertFalse(calibrator.isQualityScoresRequired());
23502         assertTrue(calibrator.isCommonAxisUsed());
23503         assertSame(this, calibrator.getListener());
23504         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23505         assertFalse(calibrator.isReady());
23506         assertFalse(calibrator.isRunning());
23507         assertNull(calibrator.getEstimatedMa());
23508         assertNull(calibrator.getEstimatedSx());
23509         assertNull(calibrator.getEstimatedSy());
23510         assertNull(calibrator.getEstimatedSz());
23511         assertNull(calibrator.getEstimatedMxy());
23512         assertNull(calibrator.getEstimatedMxz());
23513         assertNull(calibrator.getEstimatedMyx());
23514         assertNull(calibrator.getEstimatedMyz());
23515         assertNull(calibrator.getEstimatedMzx());
23516         assertNull(calibrator.getEstimatedMzy());
23517         assertNull(calibrator.getEstimatedCovariance());
23518         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23519         assertNotNull(calibrator.getGroundTruthGravityNorm());
23520         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23521         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23522         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23523                 ABSOLUTE_ERROR));
23524         final var g = new Acceleration(0.0, AccelerationUnit.G);
23525         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23526         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23527         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23528 
23529         // Force IllegalArgumentException
23530         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23531 
23532         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23533                 invalidGravityNorm, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
23534     }
23535 
23536     @Test
23537     void testConstructor201() throws WrongSizeException {
23538         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
23539 
23540         final var ba = generateBa();
23541         final var biasX = ba.getElementAtIndex(0);
23542         final var biasY = ba.getElementAtIndex(1);
23543         final var biasZ = ba.getElementAtIndex(2);
23544 
23545         final var ma = generateMaCommonAxis();
23546         final var sx = ma.getElementAt(0, 0);
23547         final var sy = ma.getElementAt(1, 1);
23548         final var sz = ma.getElementAt(2, 2);
23549         final var mxy = ma.getElementAt(0, 1);
23550         final var mxz = ma.getElementAt(0, 2);
23551         final var myx = ma.getElementAt(1, 0);
23552         final var myz = ma.getElementAt(1, 2);
23553         final var mzx = ma.getElementAt(2, 0);
23554         final var mzy = ma.getElementAt(2, 1);
23555 
23556         final var randomizer = new UniformRandomizer();
23557         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23558         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23559         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23560         final var nedPosition = new NEDPosition(latitude, longitude, height);
23561         final var nedVelocity = new NEDVelocity();
23562         final var ecefPosition = new ECEFPosition();
23563         final var ecefVelocity = new ECEFVelocity();
23564         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23565         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23566                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23567         final var gravityNorm = gravity.getNormAsAcceleration();
23568 
23569         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23570                 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
23571 
23572         // check default values
23573         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23574         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23575         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23576         final var bx1 = calibrator.getBiasXAsAcceleration();
23577         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23578         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23579         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23580         calibrator.getBiasXAsAcceleration(bx2);
23581         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23582         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23583         final var by1 = calibrator.getBiasYAsAcceleration();
23584         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23585         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23586         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23587         calibrator.getBiasYAsAcceleration(by2);
23588         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23589         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23590         final var bz1 = calibrator.getBiasZAsAcceleration();
23591         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23592         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23593         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23594         calibrator.getBiasZAsAcceleration(bz2);
23595         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23596         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23597         final var biasTriad1 = calibrator.getBiasAsTriad();
23598         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23599         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23600         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23601         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23602         final var biasTriad2 = new AccelerationTriad();
23603         calibrator.getBiasAsTriad(biasTriad2);
23604         assertEquals(biasTriad1, biasTriad2);
23605         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23606         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23607         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23608         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23609         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23610         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23611         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23612         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23613         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23614         final var bias1 = calibrator.getBias();
23615         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23616         final var bias2 = new double[3];
23617         calibrator.getBias(bias2);
23618         assertArrayEquals(bias1, bias2, 0.0);
23619         final var b1 = calibrator.getBiasAsMatrix();
23620         assertEquals(b1, ba);
23621         final var b2 = new Matrix(3, 1);
23622         calibrator.getBiasAsMatrix(b2);
23623         assertEquals(b1, b2);
23624         final var ma1 = new Matrix(3, 3);
23625         ma1.setSubmatrix(0, 0, 2, 2,
23626                 new double[]{sx, myx, mzx,
23627                         mxy, sy, mzy,
23628                         mxz, myz, sz});
23629         assertEquals(calibrator.getInitialMa(), ma1);
23630         final var ma2 = new Matrix(3, 3);
23631         calibrator.getInitialMa(ma2);
23632         assertEquals(ma1, ma2);
23633         assertSame(measurements, calibrator.getMeasurements());
23634         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23635                 calibrator.getMeasurementType());
23636         assertFalse(calibrator.isOrderedMeasurementsRequired());
23637         assertFalse(calibrator.isQualityScoresRequired());
23638         assertTrue(calibrator.isCommonAxisUsed());
23639         assertNull(calibrator.getListener());
23640         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23641         assertFalse(calibrator.isReady());
23642         assertFalse(calibrator.isRunning());
23643         assertNull(calibrator.getEstimatedMa());
23644         assertNull(calibrator.getEstimatedSx());
23645         assertNull(calibrator.getEstimatedSy());
23646         assertNull(calibrator.getEstimatedSz());
23647         assertNull(calibrator.getEstimatedMxy());
23648         assertNull(calibrator.getEstimatedMxz());
23649         assertNull(calibrator.getEstimatedMyx());
23650         assertNull(calibrator.getEstimatedMyz());
23651         assertNull(calibrator.getEstimatedMzx());
23652         assertNull(calibrator.getEstimatedMzy());
23653         assertNull(calibrator.getEstimatedCovariance());
23654         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23655         assertNotNull(calibrator.getGroundTruthGravityNorm());
23656         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23657         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23658         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23659                 ABSOLUTE_ERROR));
23660         final var g = new Acceleration(0.0, AccelerationUnit.G);
23661         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23662         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23663         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23664 
23665         // Force IllegalArgumentException
23666         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23667 
23668         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23669                 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23670     }
23671 
23672     @Test
23673     void testConstructor202() throws WrongSizeException {
23674         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
23675 
23676         final var ba = generateBa();
23677         final var biasX = ba.getElementAtIndex(0);
23678         final var biasY = ba.getElementAtIndex(1);
23679         final var biasZ = ba.getElementAtIndex(2);
23680 
23681         final var ma = generateMaCommonAxis();
23682         final var sx = ma.getElementAt(0, 0);
23683         final var sy = ma.getElementAt(1, 1);
23684         final var sz = ma.getElementAt(2, 2);
23685         final var mxy = ma.getElementAt(0, 1);
23686         final var mxz = ma.getElementAt(0, 2);
23687         final var myx = ma.getElementAt(1, 0);
23688         final var myz = ma.getElementAt(1, 2);
23689         final var mzx = ma.getElementAt(2, 0);
23690         final var mzy = ma.getElementAt(2, 1);
23691 
23692         final var randomizer = new UniformRandomizer();
23693         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23694         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23695         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23696         final var nedPosition = new NEDPosition(latitude, longitude, height);
23697         final var nedVelocity = new NEDVelocity();
23698         final var ecefPosition = new ECEFPosition();
23699         final var ecefVelocity = new ECEFVelocity();
23700         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23701         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23702                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23703         final var gravityNorm = gravity.getNormAsAcceleration();
23704 
23705         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
23706                 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
23707 
23708         // check default values
23709         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23710         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23711         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23712         final var bx1 = calibrator.getBiasXAsAcceleration();
23713         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23714         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23715         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23716         calibrator.getBiasXAsAcceleration(bx2);
23717         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23718         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23719         final var by1 = calibrator.getBiasYAsAcceleration();
23720         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23721         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23722         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23723         calibrator.getBiasYAsAcceleration(by2);
23724         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23725         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23726         final var bz1 = calibrator.getBiasZAsAcceleration();
23727         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23728         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23729         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23730         calibrator.getBiasZAsAcceleration(bz2);
23731         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23732         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23733         final var biasTriad1 = calibrator.getBiasAsTriad();
23734         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23735         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23736         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23737         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23738         final var biasTriad2 = new AccelerationTriad();
23739         calibrator.getBiasAsTriad(biasTriad2);
23740         assertEquals(biasTriad1, biasTriad2);
23741         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23742         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23743         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23744         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23745         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23746         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23747         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23748         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23749         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23750         final var bias1 = calibrator.getBias();
23751         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23752         final var bias2 = new double[3];
23753         calibrator.getBias(bias2);
23754         assertArrayEquals(bias1, bias2, 0.0);
23755         final var b1 = calibrator.getBiasAsMatrix();
23756         assertEquals(b1, ba);
23757         final var b2 = new Matrix(3, 1);
23758         calibrator.getBiasAsMatrix(b2);
23759         assertEquals(b1, b2);
23760         final var ma1 = new Matrix(3, 3);
23761         ma1.setSubmatrix(0, 0, 2, 2,
23762                 new double[]{sx, myx, mzx,
23763                         mxy, sy, mzy,
23764                         mxz, myz, sz});
23765         assertEquals(calibrator.getInitialMa(), ma1);
23766         final var ma2 = new Matrix(3, 3);
23767         calibrator.getInitialMa(ma2);
23768         assertEquals(ma1, ma2);
23769         assertSame(measurements, calibrator.getMeasurements());
23770         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23771                 calibrator.getMeasurementType());
23772         assertFalse(calibrator.isOrderedMeasurementsRequired());
23773         assertFalse(calibrator.isQualityScoresRequired());
23774         assertTrue(calibrator.isCommonAxisUsed());
23775         assertSame(this, calibrator.getListener());
23776         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
23777         assertFalse(calibrator.isReady());
23778         assertFalse(calibrator.isRunning());
23779         assertNull(calibrator.getEstimatedMa());
23780         assertNull(calibrator.getEstimatedSx());
23781         assertNull(calibrator.getEstimatedSy());
23782         assertNull(calibrator.getEstimatedSz());
23783         assertNull(calibrator.getEstimatedMxy());
23784         assertNull(calibrator.getEstimatedMxz());
23785         assertNull(calibrator.getEstimatedMyx());
23786         assertNull(calibrator.getEstimatedMyz());
23787         assertNull(calibrator.getEstimatedMzx());
23788         assertNull(calibrator.getEstimatedMzy());
23789         assertNull(calibrator.getEstimatedCovariance());
23790         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23791         assertNotNull(calibrator.getGroundTruthGravityNorm());
23792         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23793         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23794         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23795                 ABSOLUTE_ERROR));
23796         final var g = new Acceleration(0.0, AccelerationUnit.G);
23797         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23798         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23799         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23800 
23801         // Force IllegalArgumentException
23802         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23803 
23804         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23805                 invalidGravityNorm, measurements, true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
23806                 myx, myz, mzx, mzy, this));
23807     }
23808 
23809     @Test
23810     void testConstructor203() throws WrongSizeException {
23811         final var ba = generateBa();
23812         final var biasX = ba.getElementAtIndex(0);
23813         final var biasY = ba.getElementAtIndex(1);
23814         final var biasZ = ba.getElementAtIndex(2);
23815 
23816         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23817         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23818         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23819 
23820         final var ma = generateMaCommonAxis();
23821         final var sx = ma.getElementAt(0, 0);
23822         final var sy = ma.getElementAt(1, 1);
23823         final var sz = ma.getElementAt(2, 2);
23824         final var mxy = ma.getElementAt(0, 1);
23825         final var mxz = ma.getElementAt(0, 2);
23826         final var myx = ma.getElementAt(1, 0);
23827         final var myz = ma.getElementAt(1, 2);
23828         final var mzx = ma.getElementAt(2, 0);
23829         final var mzy = ma.getElementAt(2, 1);
23830 
23831         final var randomizer = new UniformRandomizer();
23832         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23833         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23834         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23835         final var nedPosition = new NEDPosition(latitude, longitude, height);
23836         final var nedVelocity = new NEDVelocity();
23837         final var ecefPosition = new ECEFPosition();
23838         final var ecefVelocity = new ECEFVelocity();
23839         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23840         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23841                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23842         final var gravityNorm = gravity.getNormAsAcceleration();
23843 
23844         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
23845                 mxy, mxz, myx, myz, mzx, mzy);
23846 
23847         // check default values
23848         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23849         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23850         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23851         final var bx1 = calibrator.getBiasXAsAcceleration();
23852         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23853         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23854         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23855         calibrator.getBiasXAsAcceleration(bx2);
23856         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23857         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23858         final var by1 = calibrator.getBiasYAsAcceleration();
23859         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23860         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23861         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23862         calibrator.getBiasYAsAcceleration(by2);
23863         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
23864         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
23865         final var bz1 = calibrator.getBiasZAsAcceleration();
23866         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
23867         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
23868         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23869         calibrator.getBiasZAsAcceleration(bz2);
23870         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
23871         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
23872         final var biasTriad1 = calibrator.getBiasAsTriad();
23873         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
23874         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
23875         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
23876         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
23877         final var biasTriad2 = new AccelerationTriad();
23878         calibrator.getBiasAsTriad(biasTriad2);
23879         assertEquals(biasTriad1, biasTriad2);
23880         assertEquals(sx, calibrator.getInitialSx(), 0.0);
23881         assertEquals(sy, calibrator.getInitialSy(), 0.0);
23882         assertEquals(sz, calibrator.getInitialSz(), 0.0);
23883         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
23884         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
23885         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
23886         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
23887         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
23888         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
23889         final var bias1 = calibrator.getBias();
23890         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23891         final var bias2 = new double[3];
23892         calibrator.getBias(bias2);
23893         assertArrayEquals(bias1, bias2, 0.0);
23894         final var b1 = calibrator.getBiasAsMatrix();
23895         assertEquals(b1, ba);
23896         final var b2 = new Matrix(3, 1);
23897         calibrator.getBiasAsMatrix(b2);
23898         assertEquals(b1, b2);
23899         final var ma1 = new Matrix(3, 3);
23900         ma1.setSubmatrix(0, 0, 2, 2,
23901                 new double[]{sx, myx, mzx,
23902                         mxy, sy, mzy,
23903                         mxz, myz, sz});
23904         assertEquals(ma1, calibrator.getInitialMa());
23905         final var ma2 = new Matrix(3, 3);
23906         calibrator.getInitialMa(ma2);
23907         assertEquals(ma1, ma2);
23908         assertNull(calibrator.getMeasurements());
23909         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
23910                 calibrator.getMeasurementType());
23911         assertFalse(calibrator.isOrderedMeasurementsRequired());
23912         assertFalse(calibrator.isQualityScoresRequired());
23913         assertFalse(calibrator.isCommonAxisUsed());
23914         assertNull(calibrator.getListener());
23915         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
23916         assertFalse(calibrator.isReady());
23917         assertFalse(calibrator.isRunning());
23918         assertNull(calibrator.getEstimatedMa());
23919         assertNull(calibrator.getEstimatedSx());
23920         assertNull(calibrator.getEstimatedSy());
23921         assertNull(calibrator.getEstimatedSz());
23922         assertNull(calibrator.getEstimatedMxy());
23923         assertNull(calibrator.getEstimatedMxz());
23924         assertNull(calibrator.getEstimatedMyx());
23925         assertNull(calibrator.getEstimatedMyz());
23926         assertNull(calibrator.getEstimatedMzx());
23927         assertNull(calibrator.getEstimatedMzy());
23928         assertNull(calibrator.getEstimatedCovariance());
23929         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
23930         assertNotNull(calibrator.getGroundTruthGravityNorm());
23931         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23932         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23933         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
23934                 ABSOLUTE_ERROR));
23935         final var g = new Acceleration(0.0, AccelerationUnit.G);
23936         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23937         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23938         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
23939 
23940         // Force IllegalArgumentException
23941         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23942 
23943         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
23944                 invalidGravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
23945     }
23946 
23947     @Test
23948     void testConstructor204() throws WrongSizeException {
23949         final var ba = generateBa();
23950         final var biasX = ba.getElementAtIndex(0);
23951         final var biasY = ba.getElementAtIndex(1);
23952         final var biasZ = ba.getElementAtIndex(2);
23953 
23954         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23955         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23956         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
23957 
23958         final var ma = generateMaCommonAxis();
23959         final var sx = ma.getElementAt(0, 0);
23960         final var sy = ma.getElementAt(1, 1);
23961         final var sz = ma.getElementAt(2, 2);
23962         final var mxy = ma.getElementAt(0, 1);
23963         final var mxz = ma.getElementAt(0, 2);
23964         final var myx = ma.getElementAt(1, 0);
23965         final var myz = ma.getElementAt(1, 2);
23966         final var mzx = ma.getElementAt(2, 0);
23967         final var mzy = ma.getElementAt(2, 1);
23968 
23969         final var randomizer = new UniformRandomizer();
23970         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
23971         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23972         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23973         final var nedPosition = new NEDPosition(latitude, longitude, height);
23974         final var nedVelocity = new NEDVelocity();
23975         final var ecefPosition = new ECEFPosition();
23976         final var ecefVelocity = new ECEFVelocity();
23977         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
23978         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23979                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23980         final var gravityNorm = gravity.getNormAsAcceleration();
23981 
23982         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bx, by, bz, sx, sy, sz,
23983                 mxy, mxz, myx, myz, mzx, mzy, this);
23984 
23985         // check default values
23986         assertEquals(biasX, calibrator.getBiasX(), 0.0);
23987         assertEquals(biasY, calibrator.getBiasY(), 0.0);
23988         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
23989         final var bx1 = calibrator.getBiasXAsAcceleration();
23990         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
23991         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
23992         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
23993         calibrator.getBiasXAsAcceleration(bx2);
23994         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
23995         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
23996         final var by1 = calibrator.getBiasYAsAcceleration();
23997         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
23998         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
23999         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24000         calibrator.getBiasYAsAcceleration(by2);
24001         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24002         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24003         final var bz1 = calibrator.getBiasZAsAcceleration();
24004         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24005         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24006         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24007         calibrator.getBiasZAsAcceleration(bz2);
24008         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24009         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24010         final var biasTriad1 = calibrator.getBiasAsTriad();
24011         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24012         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24013         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24014         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24015         final var biasTriad2 = new AccelerationTriad();
24016         calibrator.getBiasAsTriad(biasTriad2);
24017         assertEquals(biasTriad1, biasTriad2);
24018         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24019         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24020         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24021         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24022         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24023         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24024         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24025         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24026         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24027         final var bias1 = calibrator.getBias();
24028         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24029         final var bias2 = new double[3];
24030         calibrator.getBias(bias2);
24031         assertArrayEquals(bias1, bias2, 0.0);
24032         final var b1 = calibrator.getBiasAsMatrix();
24033         assertEquals(b1, ba);
24034         final var b2 = new Matrix(3, 1);
24035         calibrator.getBiasAsMatrix(b2);
24036         assertEquals(b1, b2);
24037         final var ma1 = new Matrix(3, 3);
24038         ma1.setSubmatrix(0, 0, 2, 2,
24039                 new double[]{sx, myx, mzx,
24040                         mxy, sy, mzy,
24041                         mxz, myz, sz});
24042         assertEquals(ma1, calibrator.getInitialMa());
24043         final var ma2 = new Matrix(3, 3);
24044         calibrator.getInitialMa(ma2);
24045         assertEquals(ma1, ma2);
24046         assertNull(calibrator.getMeasurements());
24047         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24048                 calibrator.getMeasurementType());
24049         assertFalse(calibrator.isOrderedMeasurementsRequired());
24050         assertFalse(calibrator.isQualityScoresRequired());
24051         assertFalse(calibrator.isCommonAxisUsed());
24052         assertSame(this, calibrator.getListener());
24053         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
24054         assertFalse(calibrator.isReady());
24055         assertFalse(calibrator.isRunning());
24056         assertNull(calibrator.getEstimatedMa());
24057         assertNull(calibrator.getEstimatedSx());
24058         assertNull(calibrator.getEstimatedSy());
24059         assertNull(calibrator.getEstimatedSz());
24060         assertNull(calibrator.getEstimatedMxy());
24061         assertNull(calibrator.getEstimatedMxz());
24062         assertNull(calibrator.getEstimatedMyx());
24063         assertNull(calibrator.getEstimatedMyz());
24064         assertNull(calibrator.getEstimatedMzx());
24065         assertNull(calibrator.getEstimatedMzy());
24066         assertNull(calibrator.getEstimatedCovariance());
24067         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24068         assertNotNull(calibrator.getGroundTruthGravityNorm());
24069         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24070         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24071         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24072                 ABSOLUTE_ERROR));
24073         final var g = new Acceleration(0.0, AccelerationUnit.G);
24074         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24075         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24076         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24077 
24078         // Force IllegalArgumentException
24079         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24080 
24081         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24082                 invalidGravityNorm, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
24083     }
24084 
24085     @Test
24086     void testConstructor205() throws WrongSizeException {
24087         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24088 
24089         final var ba = generateBa();
24090         final var biasX = ba.getElementAtIndex(0);
24091         final var biasY = ba.getElementAtIndex(1);
24092         final var biasZ = ba.getElementAtIndex(2);
24093 
24094         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24095         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24096         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24097 
24098         final var ma = generateMaCommonAxis();
24099         final var sx = ma.getElementAt(0, 0);
24100         final var sy = ma.getElementAt(1, 1);
24101         final var sz = ma.getElementAt(2, 2);
24102         final var mxy = ma.getElementAt(0, 1);
24103         final var mxz = ma.getElementAt(0, 2);
24104         final var myx = ma.getElementAt(1, 0);
24105         final var myz = ma.getElementAt(1, 2);
24106         final var mzx = ma.getElementAt(2, 0);
24107         final var mzy = ma.getElementAt(2, 1);
24108 
24109         final var randomizer = new UniformRandomizer();
24110         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24111         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24112         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24113         final var nedPosition = new NEDPosition(latitude, longitude, height);
24114         final var nedVelocity = new NEDVelocity();
24115         final var ecefPosition = new ECEFPosition();
24116         final var ecefVelocity = new ECEFVelocity();
24117         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24118         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24119                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24120         final var gravityNorm = gravity.getNormAsAcceleration();
24121 
24122         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
24123                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
24124 
24125         // check default values
24126         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24127         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24128         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24129         final var bx1 = calibrator.getBiasXAsAcceleration();
24130         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24131         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24132         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24133         calibrator.getBiasXAsAcceleration(bx2);
24134         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24135         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24136         final var by1 = calibrator.getBiasYAsAcceleration();
24137         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24138         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24139         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24140         calibrator.getBiasYAsAcceleration(by2);
24141         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24142         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24143         final var bz1 = calibrator.getBiasZAsAcceleration();
24144         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24145         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24146         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24147         calibrator.getBiasZAsAcceleration(bz2);
24148         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24149         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24150         final var biasTriad1 = calibrator.getBiasAsTriad();
24151         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24152         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24153         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24154         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24155         final var biasTriad2 = new AccelerationTriad();
24156         calibrator.getBiasAsTriad(biasTriad2);
24157         assertEquals(biasTriad1, biasTriad2);
24158         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24159         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24160         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24161         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24162         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24163         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24164         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24165         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24166         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24167         final var bias1 = calibrator.getBias();
24168         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24169         final var bias2 = new double[3];
24170         calibrator.getBias(bias2);
24171         assertArrayEquals(bias1, bias2, 0.0);
24172         final var b1 = calibrator.getBiasAsMatrix();
24173         assertEquals(b1, ba);
24174         final var b2 = new Matrix(3, 1);
24175         calibrator.getBiasAsMatrix(b2);
24176         assertEquals(b1, b2);
24177         final var ma1 = new Matrix(3, 3);
24178         ma1.setSubmatrix(0, 0, 2, 2,
24179                 new double[]{sx, myx, mzx,
24180                         mxy, sy, mzy,
24181                         mxz, myz, sz});
24182         assertEquals(calibrator.getInitialMa(), ma1);
24183         final var ma2 = new Matrix(3, 3);
24184         calibrator.getInitialMa(ma2);
24185         assertEquals(ma1, ma2);
24186         assertSame(measurements, calibrator.getMeasurements());
24187         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24188                 calibrator.getMeasurementType());
24189         assertFalse(calibrator.isOrderedMeasurementsRequired());
24190         assertFalse(calibrator.isQualityScoresRequired());
24191         assertFalse(calibrator.isCommonAxisUsed());
24192         assertNull(calibrator.getListener());
24193         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
24194         assertFalse(calibrator.isReady());
24195         assertFalse(calibrator.isRunning());
24196         assertNull(calibrator.getEstimatedMa());
24197         assertNull(calibrator.getEstimatedSx());
24198         assertNull(calibrator.getEstimatedSy());
24199         assertNull(calibrator.getEstimatedSz());
24200         assertNull(calibrator.getEstimatedMxy());
24201         assertNull(calibrator.getEstimatedMxz());
24202         assertNull(calibrator.getEstimatedMyx());
24203         assertNull(calibrator.getEstimatedMyz());
24204         assertNull(calibrator.getEstimatedMzx());
24205         assertNull(calibrator.getEstimatedMzy());
24206         assertNull(calibrator.getEstimatedCovariance());
24207         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24208         assertNotNull(calibrator.getGroundTruthGravityNorm());
24209         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24210         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24211         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24212                 ABSOLUTE_ERROR));
24213         final var g = new Acceleration(0.0, AccelerationUnit.G);
24214         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24215         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24216         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24217 
24218         // Force IllegalArgumentException
24219         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24220 
24221         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24222                 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
24223     }
24224 
24225     @Test
24226     void testConstructor206() throws WrongSizeException {
24227         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24228 
24229         final var ba = generateBa();
24230         final var biasX = ba.getElementAtIndex(0);
24231         final var biasY = ba.getElementAtIndex(1);
24232         final var biasZ = ba.getElementAtIndex(2);
24233 
24234         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24235         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24236         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24237 
24238         final var ma = generateMaCommonAxis();
24239         final var sx = ma.getElementAt(0, 0);
24240         final var sy = ma.getElementAt(1, 1);
24241         final var sz = ma.getElementAt(2, 2);
24242         final var mxy = ma.getElementAt(0, 1);
24243         final var mxz = ma.getElementAt(0, 2);
24244         final var myx = ma.getElementAt(1, 0);
24245         final var myz = ma.getElementAt(1, 2);
24246         final var mzx = ma.getElementAt(2, 0);
24247         final var mzy = ma.getElementAt(2, 1);
24248 
24249         final var randomizer = new UniformRandomizer();
24250         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24251         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24252         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24253         final var nedPosition = new NEDPosition(latitude, longitude, height);
24254         final var nedVelocity = new NEDVelocity();
24255         final var ecefPosition = new ECEFPosition();
24256         final var ecefVelocity = new ECEFVelocity();
24257         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24258         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24259                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24260         final var gravityNorm = gravity.getNormAsAcceleration();
24261 
24262         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bx, by, bz,
24263                 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
24264 
24265         // check default values
24266         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24267         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24268         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24269         final var bx1 = calibrator.getBiasXAsAcceleration();
24270         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24271         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24272         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24273         calibrator.getBiasXAsAcceleration(bx2);
24274         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24275         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24276         final var by1 = calibrator.getBiasYAsAcceleration();
24277         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24278         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24279         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24280         calibrator.getBiasYAsAcceleration(by2);
24281         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24282         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24283         final var bz1 = calibrator.getBiasZAsAcceleration();
24284         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24285         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24286         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24287         calibrator.getBiasZAsAcceleration(bz2);
24288         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24289         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24290         final var biasTriad1 = calibrator.getBiasAsTriad();
24291         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24292         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24293         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24294         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24295         final var biasTriad2 = new AccelerationTriad();
24296         calibrator.getBiasAsTriad(biasTriad2);
24297         assertEquals(biasTriad1, biasTriad2);
24298         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24299         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24300         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24301         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24302         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24303         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24304         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24305         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24306         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24307         final var bias1 = calibrator.getBias();
24308         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24309         final var bias2 = new double[3];
24310         calibrator.getBias(bias2);
24311         assertArrayEquals(bias1, bias2, 0.0);
24312         final var b1 = calibrator.getBiasAsMatrix();
24313         assertEquals(b1, ba);
24314         final var b2 = new Matrix(3, 1);
24315         calibrator.getBiasAsMatrix(b2);
24316         assertEquals(b1, b2);
24317         final var ma1 = new Matrix(3, 3);
24318         ma1.setSubmatrix(0, 0, 2, 2,
24319                 new double[]{sx, myx, mzx,
24320                         mxy, sy, mzy,
24321                         mxz, myz, sz});
24322         assertEquals(ma1, calibrator.getInitialMa());
24323         final var ma2 = new Matrix(3, 3);
24324         calibrator.getInitialMa(ma2);
24325         assertEquals(ma1, ma2);
24326         assertSame(measurements, calibrator.getMeasurements());
24327         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24328                 calibrator.getMeasurementType());
24329         assertFalse(calibrator.isOrderedMeasurementsRequired());
24330         assertFalse(calibrator.isQualityScoresRequired());
24331         assertFalse(calibrator.isCommonAxisUsed());
24332         assertSame(this, calibrator.getListener());
24333         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
24334         assertFalse(calibrator.isReady());
24335         assertFalse(calibrator.isRunning());
24336         assertNull(calibrator.getEstimatedMa());
24337         assertNull(calibrator.getEstimatedSx());
24338         assertNull(calibrator.getEstimatedSy());
24339         assertNull(calibrator.getEstimatedSz());
24340         assertNull(calibrator.getEstimatedMxy());
24341         assertNull(calibrator.getEstimatedMxz());
24342         assertNull(calibrator.getEstimatedMyx());
24343         assertNull(calibrator.getEstimatedMyz());
24344         assertNull(calibrator.getEstimatedMzx());
24345         assertNull(calibrator.getEstimatedMzy());
24346         assertNull(calibrator.getEstimatedCovariance());
24347         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24348         assertNotNull(calibrator.getGroundTruthGravityNorm());
24349         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24350         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24351         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24352                 ABSOLUTE_ERROR));
24353         final var g = new Acceleration(0.0, AccelerationUnit.G);
24354         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24355         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24356         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24357 
24358         // Force IllegalArgumentException
24359         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24360 
24361         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24362                 invalidGravityNorm, measurements, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this));
24363     }
24364 
24365     @Test
24366     void testConstructor207() throws WrongSizeException {
24367         final var ba = generateBa();
24368         final var biasX = ba.getElementAtIndex(0);
24369         final var biasY = ba.getElementAtIndex(1);
24370         final var biasZ = ba.getElementAtIndex(2);
24371 
24372         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24373         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24374         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24375 
24376         final var ma = generateMaCommonAxis();
24377         final var sx = ma.getElementAt(0, 0);
24378         final var sy = ma.getElementAt(1, 1);
24379         final var sz = ma.getElementAt(2, 2);
24380         final var mxy = ma.getElementAt(0, 1);
24381         final var mxz = ma.getElementAt(0, 2);
24382         final var myx = ma.getElementAt(1, 0);
24383         final var myz = ma.getElementAt(1, 2);
24384         final var mzx = ma.getElementAt(2, 0);
24385         final var mzy = ma.getElementAt(2, 1);
24386 
24387         final var randomizer = new UniformRandomizer();
24388         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24389         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24390         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24391         final var nedPosition = new NEDPosition(latitude, longitude, height);
24392         final var nedVelocity = new NEDVelocity();
24393         final var ecefPosition = new ECEFPosition();
24394         final var ecefVelocity = new ECEFVelocity();
24395         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24396         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24397                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24398         final var gravityNorm = gravity.getNormAsAcceleration();
24399 
24400         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
24401                 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
24402 
24403         // check default values
24404         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24405         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24406         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24407         final var bx1 = calibrator.getBiasXAsAcceleration();
24408         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24409         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24410         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24411         calibrator.getBiasXAsAcceleration(bx2);
24412         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24413         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24414         final var by1 = calibrator.getBiasYAsAcceleration();
24415         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24416         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24417         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24418         calibrator.getBiasYAsAcceleration(by2);
24419         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24420         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24421         final var bz1 = calibrator.getBiasZAsAcceleration();
24422         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24423         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24424         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24425         calibrator.getBiasZAsAcceleration(bz2);
24426         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24427         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24428         final var biasTriad1 = calibrator.getBiasAsTriad();
24429         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24430         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24431         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24432         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24433         final var biasTriad2 = new AccelerationTriad();
24434         calibrator.getBiasAsTriad(biasTriad2);
24435         assertEquals(biasTriad1, biasTriad2);
24436         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24437         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24438         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24439         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24440         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24441         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24442         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24443         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24444         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24445         final var bias1 = calibrator.getBias();
24446         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24447         final var bias2 = new double[3];
24448         calibrator.getBias(bias2);
24449         assertArrayEquals(bias1, bias2, 0.0);
24450         final var b1 = calibrator.getBiasAsMatrix();
24451         assertEquals(b1, ba);
24452         final var b2 = new Matrix(3, 1);
24453         calibrator.getBiasAsMatrix(b2);
24454         assertEquals(b1, b2);
24455         final var ma1 = new Matrix(3, 3);
24456         ma1.setSubmatrix(0, 0, 2, 2,
24457                 new double[]{sx, myx, mzx,
24458                         mxy, sy, mzy,
24459                         mxz, myz, sz});
24460         assertEquals(ma1, calibrator.getInitialMa());
24461         final var ma2 = new Matrix(3, 3);
24462         calibrator.getInitialMa(ma2);
24463         assertEquals(ma1, ma2);
24464         assertNull(calibrator.getMeasurements());
24465         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24466                 calibrator.getMeasurementType());
24467         assertFalse(calibrator.isOrderedMeasurementsRequired());
24468         assertFalse(calibrator.isQualityScoresRequired());
24469         assertTrue(calibrator.isCommonAxisUsed());
24470         assertNull(calibrator.getListener());
24471         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24472         assertFalse(calibrator.isReady());
24473         assertFalse(calibrator.isRunning());
24474         assertNull(calibrator.getEstimatedMa());
24475         assertNull(calibrator.getEstimatedSx());
24476         assertNull(calibrator.getEstimatedSy());
24477         assertNull(calibrator.getEstimatedSz());
24478         assertNull(calibrator.getEstimatedMxy());
24479         assertNull(calibrator.getEstimatedMxz());
24480         assertNull(calibrator.getEstimatedMyx());
24481         assertNull(calibrator.getEstimatedMyz());
24482         assertNull(calibrator.getEstimatedMzx());
24483         assertNull(calibrator.getEstimatedMzy());
24484         assertNull(calibrator.getEstimatedCovariance());
24485         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24486         assertNotNull(calibrator.getGroundTruthGravityNorm());
24487         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24488         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24489         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24490                 ABSOLUTE_ERROR));
24491         final var g = new Acceleration(0.0, AccelerationUnit.G);
24492         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24493         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24494         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24495 
24496         // Force IllegalArgumentException
24497         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24498 
24499         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24500                 invalidGravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy));
24501     }
24502 
24503     @Test
24504     void testConstructor208() throws WrongSizeException {
24505         final var ba = generateBa();
24506         final var biasX = ba.getElementAtIndex(0);
24507         final var biasY = ba.getElementAtIndex(1);
24508         final var biasZ = ba.getElementAtIndex(2);
24509 
24510         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24511         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24512         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24513 
24514         final var ma = generateMaCommonAxis();
24515         final var sx = ma.getElementAt(0, 0);
24516         final var sy = ma.getElementAt(1, 1);
24517         final var sz = ma.getElementAt(2, 2);
24518         final var mxy = ma.getElementAt(0, 1);
24519         final var mxz = ma.getElementAt(0, 2);
24520         final var myx = ma.getElementAt(1, 0);
24521         final var myz = ma.getElementAt(1, 2);
24522         final var mzx = ma.getElementAt(2, 0);
24523         final var mzy = ma.getElementAt(2, 1);
24524 
24525         final var randomizer = new UniformRandomizer();
24526         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24527         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24528         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24529         final var nedPosition = new NEDPosition(latitude, longitude, height);
24530         final var nedVelocity = new NEDVelocity();
24531         final var ecefPosition = new ECEFPosition();
24532         final var ecefVelocity = new ECEFVelocity();
24533         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24534         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24535                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24536         final var gravityNorm = gravity.getNormAsAcceleration();
24537 
24538         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
24539                 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
24540 
24541         // check default values
24542         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24543         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24544         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24545         final var bx1 = calibrator.getBiasXAsAcceleration();
24546         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24547         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24548         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24549         calibrator.getBiasXAsAcceleration(bx2);
24550         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24551         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24552         final var by1 = calibrator.getBiasYAsAcceleration();
24553         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24554         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24555         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24556         calibrator.getBiasYAsAcceleration(by2);
24557         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24558         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24559         final var bz1 = calibrator.getBiasZAsAcceleration();
24560         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24561         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24562         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24563         calibrator.getBiasZAsAcceleration(bz2);
24564         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24565         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24566         final var biasTriad1 = calibrator.getBiasAsTriad();
24567         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24568         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24569         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24570         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24571         final var biasTriad2 = new AccelerationTriad();
24572         calibrator.getBiasAsTriad(biasTriad2);
24573         assertEquals(biasTriad1, biasTriad2);
24574         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24575         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24576         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24577         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24578         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24579         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24580         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24581         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24582         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24583         final var bias1 = calibrator.getBias();
24584         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24585         final var bias2 = new double[3];
24586         calibrator.getBias(bias2);
24587         assertArrayEquals(bias1, bias2, 0.0);
24588         final var b1 = calibrator.getBiasAsMatrix();
24589         assertEquals(b1, ba);
24590         final var b2 = new Matrix(3, 1);
24591         calibrator.getBiasAsMatrix(b2);
24592         assertEquals(b1, b2);
24593         final var ma1 = new Matrix(3, 3);
24594         ma1.setSubmatrix(0, 0, 2, 2,
24595                 new double[]{sx, myx, mzx,
24596                         mxy, sy, mzy,
24597                         mxz, myz, sz});
24598         assertEquals(calibrator.getInitialMa(), ma1);
24599         final var ma2 = new Matrix(3, 3);
24600         calibrator.getInitialMa(ma2);
24601         assertEquals(ma1, ma2);
24602         assertNull(calibrator.getMeasurements());
24603         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24604                 calibrator.getMeasurementType());
24605         assertFalse(calibrator.isOrderedMeasurementsRequired());
24606         assertFalse(calibrator.isQualityScoresRequired());
24607         assertTrue(calibrator.isCommonAxisUsed());
24608         assertSame(this, calibrator.getListener());
24609         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24610         assertFalse(calibrator.isReady());
24611         assertFalse(calibrator.isRunning());
24612         assertNull(calibrator.getEstimatedMa());
24613         assertNull(calibrator.getEstimatedSx());
24614         assertNull(calibrator.getEstimatedSy());
24615         assertNull(calibrator.getEstimatedSz());
24616         assertNull(calibrator.getEstimatedMxy());
24617         assertNull(calibrator.getEstimatedMxz());
24618         assertNull(calibrator.getEstimatedMyx());
24619         assertNull(calibrator.getEstimatedMyz());
24620         assertNull(calibrator.getEstimatedMzx());
24621         assertNull(calibrator.getEstimatedMzy());
24622         assertNull(calibrator.getEstimatedCovariance());
24623         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24624         assertNotNull(calibrator.getGroundTruthGravityNorm());
24625         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24626         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24627         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24628                 ABSOLUTE_ERROR));
24629         final var g = new Acceleration(0.0, AccelerationUnit.G);
24630         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24631         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24632         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24633 
24634         // Force IllegalArgumentException
24635         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24636 
24637         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24638                 invalidGravityNorm, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
24639                 this));
24640     }
24641 
24642     @Test
24643     void testConstructor209() throws WrongSizeException {
24644         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24645 
24646         final var ba = generateBa();
24647         final var biasX = ba.getElementAtIndex(0);
24648         final var biasY = ba.getElementAtIndex(1);
24649         final var biasZ = ba.getElementAtIndex(2);
24650 
24651         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24652         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24653         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24654 
24655         final var ma = generateMaCommonAxis();
24656         final var sx = ma.getElementAt(0, 0);
24657         final var sy = ma.getElementAt(1, 1);
24658         final var sz = ma.getElementAt(2, 2);
24659         final var mxy = ma.getElementAt(0, 1);
24660         final var mxz = ma.getElementAt(0, 2);
24661         final var myx = ma.getElementAt(1, 0);
24662         final var myz = ma.getElementAt(1, 2);
24663         final var mzx = ma.getElementAt(2, 0);
24664         final var mzy = ma.getElementAt(2, 1);
24665 
24666         final var randomizer = new UniformRandomizer();
24667         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24668         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24669         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24670         final var nedPosition = new NEDPosition(latitude, longitude, height);
24671         final var nedVelocity = new NEDVelocity();
24672         final var ecefPosition = new ECEFPosition();
24673         final var ecefVelocity = new ECEFVelocity();
24674         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24675         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24676                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24677         final var gravityNorm = gravity.getNormAsAcceleration();
24678 
24679         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
24680                 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
24681 
24682         // check default values
24683         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24684         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24685         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24686         final var bx1 = calibrator.getBiasXAsAcceleration();
24687         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24688         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24689         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24690         calibrator.getBiasXAsAcceleration(bx2);
24691         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24692         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24693         final var by1 = calibrator.getBiasYAsAcceleration();
24694         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24695         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24696         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24697         calibrator.getBiasYAsAcceleration(by2);
24698         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24699         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24700         final var bz1 = calibrator.getBiasZAsAcceleration();
24701         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24702         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24703         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24704         calibrator.getBiasZAsAcceleration(bz2);
24705         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24706         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24707         final var biasTriad1 = calibrator.getBiasAsTriad();
24708         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24709         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24710         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24711         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24712         final var biasTriad2 = new AccelerationTriad();
24713         calibrator.getBiasAsTriad(biasTriad2);
24714         assertEquals(biasTriad1, biasTriad2);
24715         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24716         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24717         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24718         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24719         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24720         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24721         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24722         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24723         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24724         final var bias1 = calibrator.getBias();
24725         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24726         final var bias2 = new double[3];
24727         calibrator.getBias(bias2);
24728         assertArrayEquals(bias1, bias2, 0.0);
24729         final var b1 = calibrator.getBiasAsMatrix();
24730         assertEquals(b1, ba);
24731         final var b2 = new Matrix(3, 1);
24732         calibrator.getBiasAsMatrix(b2);
24733         assertEquals(b1, b2);
24734         final var ma1 = new Matrix(3, 3);
24735         ma1.setSubmatrix(0, 0, 2, 2,
24736                 new double[]{sx, myx, mzx,
24737                         mxy, sy, mzy,
24738                         mxz, myz, sz});
24739         assertEquals(calibrator.getInitialMa(), ma1);
24740         final var ma2 = new Matrix(3, 3);
24741         calibrator.getInitialMa(ma2);
24742         assertEquals(ma1, ma2);
24743         assertSame(measurements, calibrator.getMeasurements());
24744         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24745                 calibrator.getMeasurementType());
24746         assertFalse(calibrator.isOrderedMeasurementsRequired());
24747         assertFalse(calibrator.isQualityScoresRequired());
24748         assertTrue(calibrator.isCommonAxisUsed());
24749         assertNull(calibrator.getListener());
24750         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24751         assertFalse(calibrator.isReady());
24752         assertFalse(calibrator.isRunning());
24753         assertNull(calibrator.getEstimatedMa());
24754         assertNull(calibrator.getEstimatedSx());
24755         assertNull(calibrator.getEstimatedSy());
24756         assertNull(calibrator.getEstimatedSz());
24757         assertNull(calibrator.getEstimatedMxy());
24758         assertNull(calibrator.getEstimatedMxz());
24759         assertNull(calibrator.getEstimatedMyx());
24760         assertNull(calibrator.getEstimatedMyz());
24761         assertNull(calibrator.getEstimatedMzx());
24762         assertNull(calibrator.getEstimatedMzy());
24763         assertNull(calibrator.getEstimatedCovariance());
24764         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24765         assertNotNull(calibrator.getGroundTruthGravityNorm());
24766         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24767         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24768         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24769                 ABSOLUTE_ERROR));
24770         final var g = new Acceleration(0.0, AccelerationUnit.G);
24771         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24772         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24773         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24774 
24775         // Force IllegalArgumentException
24776         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24777 
24778         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24779                 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx,
24780                 mzy));
24781     }
24782 
24783     @Test
24784     void testConstructor210() throws WrongSizeException {
24785         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
24786 
24787         final var ba = generateBa();
24788         final var biasX = ba.getElementAtIndex(0);
24789         final var biasY = ba.getElementAtIndex(1);
24790         final var biasZ = ba.getElementAtIndex(2);
24791 
24792         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24793         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24794         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
24795 
24796         final var ma = generateMaCommonAxis();
24797         final var sx = ma.getElementAt(0, 0);
24798         final var sy = ma.getElementAt(1, 1);
24799         final var sz = ma.getElementAt(2, 2);
24800         final var mxy = ma.getElementAt(0, 1);
24801         final var mxz = ma.getElementAt(0, 2);
24802         final var myx = ma.getElementAt(1, 0);
24803         final var myz = ma.getElementAt(1, 2);
24804         final var mzx = ma.getElementAt(2, 0);
24805         final var mzy = ma.getElementAt(2, 1);
24806 
24807         final var randomizer = new UniformRandomizer();
24808         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24809         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24810         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24811         final var nedPosition = new NEDPosition(latitude, longitude, height);
24812         final var nedVelocity = new NEDVelocity();
24813         final var ecefPosition = new ECEFPosition();
24814         final var ecefVelocity = new ECEFVelocity();
24815         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24816         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24817                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24818         final var gravityNorm = gravity.getNormAsAcceleration();
24819 
24820         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
24821                 true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
24822 
24823         // check default values
24824         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24825         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24826         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24827         final var bx1 = calibrator.getBiasXAsAcceleration();
24828         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24829         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24830         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24831         calibrator.getBiasXAsAcceleration(bx2);
24832         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24833         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24834         final var by1 = calibrator.getBiasYAsAcceleration();
24835         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
24836         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24837         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24838         calibrator.getBiasYAsAcceleration(by2);
24839         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24840         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24841         final var bz1 = calibrator.getBiasZAsAcceleration();
24842         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
24843         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24844         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24845         calibrator.getBiasZAsAcceleration(bz2);
24846         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24847         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24848         final var biasTriad1 = calibrator.getBiasAsTriad();
24849         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24850         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24851         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24852         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24853         final var biasTriad2 = new AccelerationTriad();
24854         calibrator.getBiasAsTriad(biasTriad2);
24855         assertEquals(biasTriad1, biasTriad2);
24856         assertEquals(sx, calibrator.getInitialSx(), 0.0);
24857         assertEquals(sy, calibrator.getInitialSy(), 0.0);
24858         assertEquals(sz, calibrator.getInitialSz(), 0.0);
24859         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
24860         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
24861         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
24862         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
24863         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
24864         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
24865         final var bias1 = calibrator.getBias();
24866         assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24867         final var bias2 = new double[3];
24868         calibrator.getBias(bias2);
24869         assertArrayEquals(bias1, bias2, 0.0);
24870         final var b1 = calibrator.getBiasAsMatrix();
24871         assertEquals(b1, ba);
24872         final var b2 = new Matrix(3, 1);
24873         calibrator.getBiasAsMatrix(b2);
24874         assertEquals(b1, b2);
24875         final var ma1 = new Matrix(3, 3);
24876         ma1.setSubmatrix(0, 0, 2, 2,
24877                 new double[]{sx, myx, mzx,
24878                         mxy, sy, mzy,
24879                         mxz, myz, sz});
24880         assertEquals(ma1, calibrator.getInitialMa());
24881         final var ma2 = new Matrix(3, 3);
24882         calibrator.getInitialMa(ma2);
24883         assertEquals(ma1, ma2);
24884         assertSame(measurements, calibrator.getMeasurements());
24885         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
24886                 calibrator.getMeasurementType());
24887         assertFalse(calibrator.isOrderedMeasurementsRequired());
24888         assertFalse(calibrator.isQualityScoresRequired());
24889         assertTrue(calibrator.isCommonAxisUsed());
24890         assertSame(this, calibrator.getListener());
24891         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
24892         assertFalse(calibrator.isReady());
24893         assertFalse(calibrator.isRunning());
24894         assertNull(calibrator.getEstimatedMa());
24895         assertNull(calibrator.getEstimatedSx());
24896         assertNull(calibrator.getEstimatedSy());
24897         assertNull(calibrator.getEstimatedSz());
24898         assertNull(calibrator.getEstimatedMxy());
24899         assertNull(calibrator.getEstimatedMxz());
24900         assertNull(calibrator.getEstimatedMyx());
24901         assertNull(calibrator.getEstimatedMyz());
24902         assertNull(calibrator.getEstimatedMzx());
24903         assertNull(calibrator.getEstimatedMzy());
24904         assertNull(calibrator.getEstimatedCovariance());
24905         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
24906         assertNotNull(calibrator.getGroundTruthGravityNorm());
24907         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24908         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24909         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
24910                 ABSOLUTE_ERROR));
24911         final var g = new Acceleration(0.0, AccelerationUnit.G);
24912         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24913         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24914         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
24915 
24916         // Force IllegalArgumentException
24917         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24918 
24919         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
24920                 invalidGravityNorm, measurements, true, bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx,
24921                 mzy, this));
24922     }
24923 
24924     @Test
24925     void testConstructor211() throws WrongSizeException {
24926         final var ba = generateBa();
24927         final var bias = ba.getBuffer();
24928         final var biasX = ba.getElementAtIndex(0);
24929         final var biasY = ba.getElementAtIndex(1);
24930         final var biasZ = ba.getElementAtIndex(2);
24931 
24932         final var randomizer = new UniformRandomizer();
24933         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
24934         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24935         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24936         final var nedPosition = new NEDPosition(latitude, longitude, height);
24937         final var nedVelocity = new NEDVelocity();
24938         final var ecefPosition = new ECEFPosition();
24939         final var ecefVelocity = new ECEFVelocity();
24940         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
24941         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24942                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24943         final var gravityNorm = gravity.getNormAsAcceleration();
24944 
24945         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias);
24946 
24947         // check default values
24948         assertEquals(biasX, calibrator.getBiasX(), 0.0);
24949         assertEquals(biasY, calibrator.getBiasY(), 0.0);
24950         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
24951         final var bx1 = calibrator.getBiasXAsAcceleration();
24952         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
24953         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
24954         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24955         calibrator.getBiasXAsAcceleration(bx2);
24956         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
24957         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
24958         final var by1 = calibrator.getBiasYAsAcceleration();
24959         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24960         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
24961         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24962         calibrator.getBiasYAsAcceleration(by2);
24963         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
24964         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
24965         final var bz1 = calibrator.getBiasZAsAcceleration();
24966         assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24967         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
24968         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
24969         calibrator.getBiasZAsAcceleration(bz2);
24970         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
24971         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
24972         final var biasTriad1 = calibrator.getBiasAsTriad();
24973         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
24974         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
24975         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
24976         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
24977         final var biasTriad2 = new AccelerationTriad();
24978         calibrator.getBiasAsTriad(biasTriad2);
24979         assertEquals(biasTriad1, biasTriad2);
24980         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
24981         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
24982         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
24983         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
24984         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
24985         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
24986         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
24987         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
24988         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
24989         final var bias1 = calibrator.getBias();
24990         assertArrayEquals(bias1, bias, 0.0);
24991         final var bias2 = new double[3];
24992         calibrator.getBias(bias2);
24993         assertArrayEquals(bias1, bias2, 0.0);
24994         final var b1 = calibrator.getBiasAsMatrix();
24995         assertEquals(b1, ba);
24996         final var b2 = new Matrix(3, 1);
24997         calibrator.getBiasAsMatrix(b2);
24998         assertEquals(b1, b2);
24999         final var ma1 = calibrator.getInitialMa();
25000         assertEquals(new Matrix(3, 3), ma1);
25001         final var ma2 = new Matrix(3, 3);
25002         calibrator.getInitialMa(ma2);
25003         assertEquals(ma1, ma2);
25004         assertNull(calibrator.getMeasurements());
25005         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25006                 calibrator.getMeasurementType());
25007         assertFalse(calibrator.isOrderedMeasurementsRequired());
25008         assertFalse(calibrator.isQualityScoresRequired());
25009         assertFalse(calibrator.isCommonAxisUsed());
25010         assertNull(calibrator.getListener());
25011         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25012         assertFalse(calibrator.isReady());
25013         assertFalse(calibrator.isRunning());
25014         assertNull(calibrator.getEstimatedMa());
25015         assertNull(calibrator.getEstimatedSx());
25016         assertNull(calibrator.getEstimatedSy());
25017         assertNull(calibrator.getEstimatedSz());
25018         assertNull(calibrator.getEstimatedMxy());
25019         assertNull(calibrator.getEstimatedMxz());
25020         assertNull(calibrator.getEstimatedMyx());
25021         assertNull(calibrator.getEstimatedMyz());
25022         assertNull(calibrator.getEstimatedMzx());
25023         assertNull(calibrator.getEstimatedMzy());
25024         assertNull(calibrator.getEstimatedCovariance());
25025         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25026         assertNotNull(calibrator.getGroundTruthGravityNorm());
25027         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25028         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25029         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25030                 ABSOLUTE_ERROR));
25031         final var g = new Acceleration(0.0, AccelerationUnit.G);
25032         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25033         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25034         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25035 
25036         // Force IllegalArgumentException
25037         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25038 
25039         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25040                 invalidGravityNorm, bias));
25041         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25042                 gravityNorm, new double[1]));
25043     }
25044 
25045     @Test
25046     void testConstructor212() throws WrongSizeException {
25047         final var ba = generateBa();
25048         final var bias = ba.getBuffer();
25049         final var biasX = ba.getElementAtIndex(0);
25050         final var biasY = ba.getElementAtIndex(1);
25051         final var biasZ = ba.getElementAtIndex(2);
25052 
25053         final var randomizer = new UniformRandomizer();
25054         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25055         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25056         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25057         final var nedPosition = new NEDPosition(latitude, longitude, height);
25058         final var nedVelocity = new NEDVelocity();
25059         final var ecefPosition = new ECEFPosition();
25060         final var ecefVelocity = new ECEFVelocity();
25061         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25062         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25063                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25064         final var gravityNorm = gravity.getNormAsAcceleration();
25065 
25066         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, bias, this);
25067 
25068         // check default values
25069         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25070         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25071         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25072         final var bx1 = calibrator.getBiasXAsAcceleration();
25073         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25074         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25075         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25076         calibrator.getBiasXAsAcceleration(bx2);
25077         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25078         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25079         final var by1 = calibrator.getBiasYAsAcceleration();
25080         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25081         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25082         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25083         calibrator.getBiasYAsAcceleration(by2);
25084         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25085         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25086         final var bz1 = calibrator.getBiasZAsAcceleration();
25087         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25088         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25089         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25090         calibrator.getBiasZAsAcceleration(bz2);
25091         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25092         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25093         final var biasTriad1 = calibrator.getBiasAsTriad();
25094         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25095         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25096         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25097         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25098         final var biasTriad2 = new AccelerationTriad();
25099         calibrator.getBiasAsTriad(biasTriad2);
25100         assertEquals(biasTriad1, biasTriad2);
25101         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25102         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25103         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25104         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25105         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25106         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25107         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25108         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25109         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25110         final var bias1 = calibrator.getBias();
25111         assertArrayEquals(bias1, bias, 0.0);
25112         final var bias2 = new double[3];
25113         calibrator.getBias(bias2);
25114         assertArrayEquals(bias1, bias2, 0.0);
25115         final var b1 = calibrator.getBiasAsMatrix();
25116         assertEquals(b1, ba);
25117         final var b2 = new Matrix(3, 1);
25118         calibrator.getBiasAsMatrix(b2);
25119         assertEquals(b1, b2);
25120         final var ma1 = calibrator.getInitialMa();
25121         assertEquals(new Matrix(3, 3), ma1);
25122         final var ma2 = new Matrix(3, 3);
25123         calibrator.getInitialMa(ma2);
25124         assertEquals(ma1, ma2);
25125         assertNull(calibrator.getMeasurements());
25126         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25127                 calibrator.getMeasurementType());
25128         assertFalse(calibrator.isOrderedMeasurementsRequired());
25129         assertFalse(calibrator.isQualityScoresRequired());
25130         assertFalse(calibrator.isCommonAxisUsed());
25131         assertSame(this, calibrator.getListener());
25132         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25133         assertFalse(calibrator.isReady());
25134         assertFalse(calibrator.isRunning());
25135         assertNull(calibrator.getEstimatedMa());
25136         assertNull(calibrator.getEstimatedSx());
25137         assertNull(calibrator.getEstimatedSy());
25138         assertNull(calibrator.getEstimatedSz());
25139         assertNull(calibrator.getEstimatedMxy());
25140         assertNull(calibrator.getEstimatedMxz());
25141         assertNull(calibrator.getEstimatedMyx());
25142         assertNull(calibrator.getEstimatedMyz());
25143         assertNull(calibrator.getEstimatedMzx());
25144         assertNull(calibrator.getEstimatedMzy());
25145         assertNull(calibrator.getEstimatedCovariance());
25146         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25147         assertNotNull(calibrator.getGroundTruthGravityNorm());
25148         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25149         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25150         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25151                 ABSOLUTE_ERROR));
25152         final var g = new Acceleration(0.0, AccelerationUnit.G);
25153         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25154         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25155         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25156 
25157         // Force IllegalArgumentException
25158         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25159 
25160         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25161                 invalidGravityNorm, bias, this));
25162         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25163                 gravityNorm, new double[1], this));
25164     }
25165 
25166     @Test
25167     void testConstructor213() throws WrongSizeException {
25168         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25169 
25170         final var ba = generateBa();
25171         final var bias = ba.getBuffer();
25172         final var biasX = ba.getElementAtIndex(0);
25173         final var biasY = ba.getElementAtIndex(1);
25174         final var biasZ = ba.getElementAtIndex(2);
25175 
25176         final var randomizer = new UniformRandomizer();
25177         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25178         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25179         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25180         final var nedPosition = new NEDPosition(latitude, longitude, height);
25181         final var nedVelocity = new NEDVelocity();
25182         final var ecefPosition = new ECEFPosition();
25183         final var ecefVelocity = new ECEFVelocity();
25184         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25185         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25186                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25187         final var gravityNorm = gravity.getNormAsAcceleration();
25188 
25189         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias);
25190 
25191         // check default values
25192         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25193         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25194         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25195         final var bx1 = calibrator.getBiasXAsAcceleration();
25196         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25197         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25198         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25199         calibrator.getBiasXAsAcceleration(bx2);
25200         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25201         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25202         final var by1 = calibrator.getBiasYAsAcceleration();
25203         assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25204         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25205         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25206         calibrator.getBiasYAsAcceleration(by2);
25207         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25208         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25209         final var bz1 = calibrator.getBiasZAsAcceleration();
25210         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25211         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25212         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25213         calibrator.getBiasZAsAcceleration(bz2);
25214         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25215         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25216         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25217         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25218         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25219         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25220         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25221         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25222         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25223         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25224         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25225         final var bias1 = calibrator.getBias();
25226         assertArrayEquals(bias1, bias, 0.0);
25227         final var bias2 = new double[3];
25228         calibrator.getBias(bias2);
25229         assertArrayEquals(bias1, bias2, 0.0);
25230         final var b1 = calibrator.getBiasAsMatrix();
25231         assertEquals(b1, ba);
25232         final var b2 = new Matrix(3, 1);
25233         calibrator.getBiasAsMatrix(b2);
25234         assertEquals(b1, b2);
25235         final var ma1 = calibrator.getInitialMa();
25236         assertEquals(new Matrix(3, 3), ma1);
25237         final var ma2 = new Matrix(3, 3);
25238         calibrator.getInitialMa(ma2);
25239         assertEquals(ma1, ma2);
25240         assertSame(measurements, calibrator.getMeasurements());
25241         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25242                 calibrator.getMeasurementType());
25243         assertFalse(calibrator.isOrderedMeasurementsRequired());
25244         assertFalse(calibrator.isQualityScoresRequired());
25245         assertFalse(calibrator.isCommonAxisUsed());
25246         assertNull(calibrator.getListener());
25247         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25248         assertFalse(calibrator.isReady());
25249         assertFalse(calibrator.isRunning());
25250         assertNull(calibrator.getEstimatedMa());
25251         assertNull(calibrator.getEstimatedSx());
25252         assertNull(calibrator.getEstimatedSy());
25253         assertNull(calibrator.getEstimatedSz());
25254         assertNull(calibrator.getEstimatedMxy());
25255         assertNull(calibrator.getEstimatedMxz());
25256         assertNull(calibrator.getEstimatedMyx());
25257         assertNull(calibrator.getEstimatedMyz());
25258         assertNull(calibrator.getEstimatedMzx());
25259         assertNull(calibrator.getEstimatedMzy());
25260         assertNull(calibrator.getEstimatedCovariance());
25261         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25262         assertNotNull(calibrator.getGroundTruthGravityNorm());
25263         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25264         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25265         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25266                 ABSOLUTE_ERROR));
25267         final var g = new Acceleration(0.0, AccelerationUnit.G);
25268         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25269         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25270         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25271 
25272         // Force IllegalArgumentException
25273         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25274 
25275         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25276                 invalidGravityNorm, measurements, bias));
25277         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25278                 gravityNorm, measurements, new double[1]));
25279     }
25280 
25281     @Test
25282     void testConstructor214() throws WrongSizeException {
25283         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25284 
25285         final var ba = generateBa();
25286         final var bias = ba.getBuffer();
25287         final var biasX = ba.getElementAtIndex(0);
25288         final var biasY = ba.getElementAtIndex(1);
25289         final var biasZ = ba.getElementAtIndex(2);
25290 
25291         final var randomizer = new UniformRandomizer();
25292         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25293         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25294         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25295         final var nedPosition = new NEDPosition(latitude, longitude, height);
25296         final var nedVelocity = new NEDVelocity();
25297         final var ecefPosition = new ECEFPosition();
25298         final var ecefVelocity = new ECEFVelocity();
25299         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25300         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25301                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25302         final var gravityNorm = gravity.getNormAsAcceleration();
25303 
25304         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, bias,
25305                 this);
25306 
25307         // check default values
25308         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25309         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25310         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25311         final var bx1 = calibrator.getBiasXAsAcceleration();
25312         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25313         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25314         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25315         calibrator.getBiasXAsAcceleration(bx2);
25316         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25317         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25318         final var by1 = calibrator.getBiasYAsAcceleration();
25319         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25320         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25321         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25322         calibrator.getBiasYAsAcceleration(by2);
25323         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25324         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25325         final var bz1 = calibrator.getBiasZAsAcceleration();
25326         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25327         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25328         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25329         calibrator.getBiasZAsAcceleration(bz2);
25330         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25331         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25332         final var biasTriad1 = calibrator.getBiasAsTriad();
25333         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25334         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25335         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25336         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25337         final var biasTriad2 = new AccelerationTriad();
25338         calibrator.getBiasAsTriad(biasTriad2);
25339         assertEquals(biasTriad1, biasTriad2);
25340         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25341         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25342         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25343         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25344         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25345         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25346         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25347         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25348         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25349         final var bias1 = calibrator.getBias();
25350         assertArrayEquals(bias1, bias, 0.0);
25351         final var bias2 = new double[3];
25352         calibrator.getBias(bias2);
25353         assertArrayEquals(bias1, bias2, 0.0);
25354         final var b1 = calibrator.getBiasAsMatrix();
25355         assertEquals(b1, ba);
25356         final var b2 = new Matrix(3, 1);
25357         calibrator.getBiasAsMatrix(b2);
25358         assertEquals(b1, b2);
25359         final var ma1 = calibrator.getInitialMa();
25360         assertEquals(new Matrix(3, 3), ma1);
25361         final var ma2 = new Matrix(3, 3);
25362         calibrator.getInitialMa(ma2);
25363         assertEquals(ma1, ma2);
25364         assertSame(measurements, calibrator.getMeasurements());
25365         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25366                 calibrator.getMeasurementType());
25367         assertFalse(calibrator.isOrderedMeasurementsRequired());
25368         assertFalse(calibrator.isQualityScoresRequired());
25369         assertFalse(calibrator.isCommonAxisUsed());
25370         assertSame(this, calibrator.getListener());
25371         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25372         assertFalse(calibrator.isReady());
25373         assertFalse(calibrator.isRunning());
25374         assertNull(calibrator.getEstimatedMa());
25375         assertNull(calibrator.getEstimatedSx());
25376         assertNull(calibrator.getEstimatedSy());
25377         assertNull(calibrator.getEstimatedSz());
25378         assertNull(calibrator.getEstimatedMxy());
25379         assertNull(calibrator.getEstimatedMxz());
25380         assertNull(calibrator.getEstimatedMyx());
25381         assertNull(calibrator.getEstimatedMyz());
25382         assertNull(calibrator.getEstimatedMzx());
25383         assertNull(calibrator.getEstimatedMzy());
25384         assertNull(calibrator.getEstimatedCovariance());
25385         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25386         assertNotNull(calibrator.getGroundTruthGravityNorm());
25387         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25388         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25389         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25390                 ABSOLUTE_ERROR));
25391         final var g = new Acceleration(0.0, AccelerationUnit.G);
25392         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25393         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25394         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25395 
25396         // Force IllegalArgumentException
25397         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25398 
25399         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25400                 invalidGravityNorm, measurements, bias, this));
25401         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25402                 gravityNorm, measurements, new double[1], this));
25403     }
25404 
25405     @Test
25406     void testConstructor215() throws WrongSizeException {
25407         final var ba = generateBa();
25408         final var bias = ba.getBuffer();
25409         final var biasX = ba.getElementAtIndex(0);
25410         final var biasY = ba.getElementAtIndex(1);
25411         final var biasZ = ba.getElementAtIndex(2);
25412 
25413         final var randomizer = new UniformRandomizer();
25414         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25415         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25416         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25417         final var nedPosition = new NEDPosition(latitude, longitude, height);
25418         final var nedVelocity = new NEDVelocity();
25419         final var ecefPosition = new ECEFPosition();
25420         final var ecefVelocity = new ECEFVelocity();
25421         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25422         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25423                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25424         final var gravityNorm = gravity.getNormAsAcceleration();
25425 
25426         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
25427                 bias);
25428 
25429         // check default values
25430         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25431         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25432         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25433         final var bx1 = calibrator.getBiasXAsAcceleration();
25434         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25435         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25436         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25437         calibrator.getBiasXAsAcceleration(bx2);
25438         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25439         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25440         final var by1 = calibrator.getBiasYAsAcceleration();
25441         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25442         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25443         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25444         calibrator.getBiasYAsAcceleration(by2);
25445         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25446         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25447         final var bz1 = calibrator.getBiasZAsAcceleration();
25448         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25449         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25450         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25451         calibrator.getBiasZAsAcceleration(bz2);
25452         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25453         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25454         final var biasTriad1 = calibrator.getBiasAsTriad();
25455         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25456         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25457         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25458         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25459         final var biasTriad2 = new AccelerationTriad();
25460         calibrator.getBiasAsTriad(biasTriad2);
25461         assertEquals(biasTriad1, biasTriad2);
25462         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25463         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25464         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25465         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25466         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25467         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25468         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25469         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25470         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25471         final var bias1 = calibrator.getBias();
25472         assertArrayEquals(bias1, bias, 0.0);
25473         final var bias2 = new double[3];
25474         calibrator.getBias(bias2);
25475         assertArrayEquals(bias1, bias2, 0.0);
25476         final var b1 = calibrator.getBiasAsMatrix();
25477         assertEquals(b1, ba);
25478         final var b2 = new Matrix(3, 1);
25479         calibrator.getBiasAsMatrix(b2);
25480         assertEquals(b1, b2);
25481         final var ma1 = calibrator.getInitialMa();
25482         assertEquals(new Matrix(3, 3), ma1);
25483         final var ma2 = new Matrix(3, 3);
25484         calibrator.getInitialMa(ma2);
25485         assertEquals(ma1, ma2);
25486         assertNull(calibrator.getMeasurements());
25487         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25488                 calibrator.getMeasurementType());
25489         assertFalse(calibrator.isOrderedMeasurementsRequired());
25490         assertFalse(calibrator.isQualityScoresRequired());
25491         assertTrue(calibrator.isCommonAxisUsed());
25492         assertNull(calibrator.getListener());
25493         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25494         assertFalse(calibrator.isReady());
25495         assertFalse(calibrator.isRunning());
25496         assertNull(calibrator.getEstimatedMa());
25497         assertNull(calibrator.getEstimatedSx());
25498         assertNull(calibrator.getEstimatedSy());
25499         assertNull(calibrator.getEstimatedSz());
25500         assertNull(calibrator.getEstimatedMxy());
25501         assertNull(calibrator.getEstimatedMxz());
25502         assertNull(calibrator.getEstimatedMyx());
25503         assertNull(calibrator.getEstimatedMyz());
25504         assertNull(calibrator.getEstimatedMzx());
25505         assertNull(calibrator.getEstimatedMzy());
25506         assertNull(calibrator.getEstimatedCovariance());
25507         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25508         assertNotNull(calibrator.getGroundTruthGravityNorm());
25509         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25510         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25511         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25512                 ABSOLUTE_ERROR));
25513         final var g = new Acceleration(0.0, AccelerationUnit.G);
25514         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25515         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25516         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25517 
25518         // Force IllegalArgumentException
25519         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25520 
25521         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25522                 invalidGravityNorm, true, bias));
25523         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25524                 gravityNorm, true, new double[1]));
25525     }
25526 
25527     @Test
25528     void testConstructor216() throws WrongSizeException {
25529         final var ba = generateBa();
25530         final var bias = ba.getBuffer();
25531         final var biasX = ba.getElementAtIndex(0);
25532         final var biasY = ba.getElementAtIndex(1);
25533         final var biasZ = ba.getElementAtIndex(2);
25534 
25535         final var randomizer = new UniformRandomizer();
25536         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25537         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25538         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25539         final var nedPosition = new NEDPosition(latitude, longitude, height);
25540         final var nedVelocity = new NEDVelocity();
25541         final var ecefPosition = new ECEFPosition();
25542         final var ecefVelocity = new ECEFVelocity();
25543         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25544         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25545                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25546         final var gravityNorm = gravity.getNormAsAcceleration();
25547 
25548         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true,
25549                 bias, this);
25550 
25551         // check default values
25552         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25553         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25554         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25555         final var bx1 = calibrator.getBiasXAsAcceleration();
25556         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25557         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25558         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25559         calibrator.getBiasXAsAcceleration(bx2);
25560         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25561         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25562         final var by1 = calibrator.getBiasYAsAcceleration();
25563         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25564         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25565         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25566         calibrator.getBiasYAsAcceleration(by2);
25567         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25568         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25569         final var bz1 = calibrator.getBiasZAsAcceleration();
25570         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25571         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25572         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25573         calibrator.getBiasZAsAcceleration(bz2);
25574         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25575         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25576         final var biasTriad1 = calibrator.getBiasAsTriad();
25577         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25578         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25579         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25580         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25581         final var biasTriad2 = new AccelerationTriad();
25582         calibrator.getBiasAsTriad(biasTriad2);
25583         assertEquals(biasTriad1, biasTriad2);
25584         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25585         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25586         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25587         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25588         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25589         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25590         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25591         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25592         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25593         final var bias1 = calibrator.getBias();
25594         assertArrayEquals(bias1, bias, 0.0);
25595         final var bias2 = new double[3];
25596         calibrator.getBias(bias2);
25597         assertArrayEquals(bias1, bias2, 0.0);
25598         final var b1 = calibrator.getBiasAsMatrix();
25599         assertEquals(b1, ba);
25600         final var b2 = new Matrix(3, 1);
25601         calibrator.getBiasAsMatrix(b2);
25602         assertEquals(b1, b2);
25603         final var ma1 = calibrator.getInitialMa();
25604         assertEquals(new Matrix(3, 3), ma1);
25605         final var ma2 = new Matrix(3, 3);
25606         calibrator.getInitialMa(ma2);
25607         assertEquals(ma1, ma2);
25608         assertNull(calibrator.getMeasurements());
25609         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25610                 calibrator.getMeasurementType());
25611         assertFalse(calibrator.isOrderedMeasurementsRequired());
25612         assertFalse(calibrator.isQualityScoresRequired());
25613         assertTrue(calibrator.isCommonAxisUsed());
25614         assertSame(this, calibrator.getListener());
25615         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25616         assertFalse(calibrator.isReady());
25617         assertFalse(calibrator.isRunning());
25618         assertNull(calibrator.getEstimatedMa());
25619         assertNull(calibrator.getEstimatedSx());
25620         assertNull(calibrator.getEstimatedSy());
25621         assertNull(calibrator.getEstimatedSz());
25622         assertNull(calibrator.getEstimatedMxy());
25623         assertNull(calibrator.getEstimatedMxz());
25624         assertNull(calibrator.getEstimatedMyx());
25625         assertNull(calibrator.getEstimatedMyz());
25626         assertNull(calibrator.getEstimatedMzx());
25627         assertNull(calibrator.getEstimatedMzy());
25628         assertNull(calibrator.getEstimatedCovariance());
25629         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25630         assertNotNull(calibrator.getGroundTruthGravityNorm());
25631         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25632         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25633         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25634                 ABSOLUTE_ERROR));
25635         final var g = new Acceleration(0.0, AccelerationUnit.G);
25636         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25637         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25638         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25639 
25640         // Force IllegalArgumentException
25641         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25642 
25643         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25644                 invalidGravityNorm, true, bias, this));
25645         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25646                 gravityNorm, true, new double[1], this));
25647     }
25648 
25649     @Test
25650     void testConstructor217() throws WrongSizeException {
25651         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25652 
25653         final var ba = generateBa();
25654         final var bias = ba.getBuffer();
25655         final var biasX = ba.getElementAtIndex(0);
25656         final var biasY = ba.getElementAtIndex(1);
25657         final var biasZ = ba.getElementAtIndex(2);
25658 
25659         final var randomizer = new UniformRandomizer();
25660         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25661         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25662         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25663         final var nedPosition = new NEDPosition(latitude, longitude, height);
25664         final var nedVelocity = new NEDVelocity();
25665         final var ecefPosition = new ECEFPosition();
25666         final var ecefVelocity = new ECEFVelocity();
25667         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25668         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25669                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25670         final var gravityNorm = gravity.getNormAsAcceleration();
25671 
25672         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
25673                 true, bias);
25674 
25675         // check default values
25676         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25677         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25678         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25679         final var bx1 = calibrator.getBiasXAsAcceleration();
25680         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25681         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25682         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25683         calibrator.getBiasXAsAcceleration(bx2);
25684         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25685         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25686         final var by1 = calibrator.getBiasYAsAcceleration();
25687         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25688         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25689         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25690         calibrator.getBiasYAsAcceleration(by2);
25691         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25692         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25693         final var bz1 = calibrator.getBiasZAsAcceleration();
25694         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25695         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25696         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25697         calibrator.getBiasZAsAcceleration(bz2);
25698         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25699         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25700         final var biasTriad1 = calibrator.getBiasAsTriad();
25701         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25702         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25703         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25704         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25705         final var biasTriad2 = new AccelerationTriad();
25706         calibrator.getBiasAsTriad(biasTriad2);
25707         assertEquals(biasTriad1, biasTriad2);
25708         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25709         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25710         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25711         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25712         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25713         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25714         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25715         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25716         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25717         final var bias1 = calibrator.getBias();
25718         assertArrayEquals(bias1, bias, 0.0);
25719         final var bias2 = new double[3];
25720         calibrator.getBias(bias2);
25721         assertArrayEquals(bias1, bias2, 0.0);
25722         final var b1 = calibrator.getBiasAsMatrix();
25723         assertEquals(b1, ba);
25724         final var b2 = new Matrix(3, 1);
25725         calibrator.getBiasAsMatrix(b2);
25726         assertEquals(b1, b2);
25727         final var ma1 = calibrator.getInitialMa();
25728         assertEquals(new Matrix(3, 3), ma1);
25729         final var ma2 = new Matrix(3, 3);
25730         calibrator.getInitialMa(ma2);
25731         assertEquals(ma1, ma2);
25732         assertSame(measurements, calibrator.getMeasurements());
25733         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25734                 calibrator.getMeasurementType());
25735         assertFalse(calibrator.isOrderedMeasurementsRequired());
25736         assertFalse(calibrator.isQualityScoresRequired());
25737         assertTrue(calibrator.isCommonAxisUsed());
25738         assertNull(calibrator.getListener());
25739         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25740         assertFalse(calibrator.isReady());
25741         assertFalse(calibrator.isRunning());
25742         assertNull(calibrator.getEstimatedMa());
25743         assertNull(calibrator.getEstimatedSx());
25744         assertNull(calibrator.getEstimatedSy());
25745         assertNull(calibrator.getEstimatedSz());
25746         assertNull(calibrator.getEstimatedMxy());
25747         assertNull(calibrator.getEstimatedMxz());
25748         assertNull(calibrator.getEstimatedMyx());
25749         assertNull(calibrator.getEstimatedMyz());
25750         assertNull(calibrator.getEstimatedMzx());
25751         assertNull(calibrator.getEstimatedMzy());
25752         assertNull(calibrator.getEstimatedCovariance());
25753         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25754         assertNotNull(calibrator.getGroundTruthGravityNorm());
25755         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25756         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25757         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25758                 ABSOLUTE_ERROR));
25759         final var g = new Acceleration(0.0, AccelerationUnit.G);
25760         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25761         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25762         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25763 
25764         // Force IllegalArgumentException
25765         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25766 
25767         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25768                 invalidGravityNorm, measurements, true, bias));
25769         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25770                 gravityNorm, measurements, true, new double[1]));
25771     }
25772 
25773     @Test
25774     void testConstructor218() throws WrongSizeException {
25775         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
25776 
25777         final var ba = generateBa();
25778         final var bias = ba.getBuffer();
25779         final var biasX = ba.getElementAtIndex(0);
25780         final var biasY = ba.getElementAtIndex(1);
25781         final var biasZ = ba.getElementAtIndex(2);
25782 
25783         final var randomizer = new UniformRandomizer();
25784         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25785         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25786         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25787         final var nedPosition = new NEDPosition(latitude, longitude, height);
25788         final var nedVelocity = new NEDVelocity();
25789         final var ecefPosition = new ECEFPosition();
25790         final var ecefVelocity = new ECEFVelocity();
25791         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25792         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25793                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25794         final var gravityNorm = gravity.getNormAsAcceleration();
25795 
25796         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
25797                 true, bias, this);
25798 
25799         // check default values
25800         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25801         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25802         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25803         final var bx1 = calibrator.getBiasXAsAcceleration();
25804         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25805         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25806         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25807         calibrator.getBiasXAsAcceleration(bx2);
25808         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25809         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25810         final var by1 = calibrator.getBiasYAsAcceleration();
25811         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25812         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25813         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25814         calibrator.getBiasYAsAcceleration(by2);
25815         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25816         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25817         final var bz1 = calibrator.getBiasZAsAcceleration();
25818         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25819         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25820         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25821         calibrator.getBiasZAsAcceleration(bz2);
25822         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25823         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25824         final var biasTriad1 = calibrator.getBiasAsTriad();
25825         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25826         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25827         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25828         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25829         final var biasTriad2 = new AccelerationTriad();
25830         calibrator.getBiasAsTriad(biasTriad2);
25831         assertEquals(biasTriad1, biasTriad2);
25832         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25833         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25834         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25835         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25836         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25837         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25838         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25839         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25840         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25841         final var bias1 = calibrator.getBias();
25842         assertArrayEquals(bias1, bias, 0.0);
25843         final var bias2 = new double[3];
25844         calibrator.getBias(bias2);
25845         assertArrayEquals(bias1, bias2, 0.0);
25846         final var b1 = calibrator.getBiasAsMatrix();
25847         assertEquals(b1, ba);
25848         final var b2 = new Matrix(3, 1);
25849         calibrator.getBiasAsMatrix(b2);
25850         assertEquals(b1, b2);
25851         final var ma1 = calibrator.getInitialMa();
25852         assertEquals(new Matrix(3, 3), ma1);
25853         final var ma2 = new Matrix(3, 3);
25854         calibrator.getInitialMa(ma2);
25855         assertEquals(ma1, ma2);
25856         assertSame(measurements, calibrator.getMeasurements());
25857         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25858                 calibrator.getMeasurementType());
25859         assertFalse(calibrator.isOrderedMeasurementsRequired());
25860         assertFalse(calibrator.isQualityScoresRequired());
25861         assertTrue(calibrator.isCommonAxisUsed());
25862         assertSame(this, calibrator.getListener());
25863         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
25864         assertFalse(calibrator.isReady());
25865         assertFalse(calibrator.isRunning());
25866         assertNull(calibrator.getEstimatedMa());
25867         assertNull(calibrator.getEstimatedSx());
25868         assertNull(calibrator.getEstimatedSy());
25869         assertNull(calibrator.getEstimatedSz());
25870         assertNull(calibrator.getEstimatedMxy());
25871         assertNull(calibrator.getEstimatedMxz());
25872         assertNull(calibrator.getEstimatedMyx());
25873         assertNull(calibrator.getEstimatedMyz());
25874         assertNull(calibrator.getEstimatedMzx());
25875         assertNull(calibrator.getEstimatedMzy());
25876         assertNull(calibrator.getEstimatedCovariance());
25877         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25878         assertNotNull(calibrator.getGroundTruthGravityNorm());
25879         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25880         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25881         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
25882                 ABSOLUTE_ERROR));
25883         final var g = new Acceleration(0.0, AccelerationUnit.G);
25884         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25885         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25886         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
25887 
25888         // Force IllegalArgumentException
25889         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25890 
25891         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25892                 invalidGravityNorm, measurements, true, bias, this));
25893         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
25894                 gravityNorm, measurements, true, new double[1], this));
25895     }
25896 
25897     @Test
25898     void testConstructor219() throws WrongSizeException {
25899         final var ba = generateBa();
25900         final var bias = ba.getBuffer();
25901         final var biasX = ba.getElementAtIndex(0);
25902         final var biasY = ba.getElementAtIndex(1);
25903         final var biasZ = ba.getElementAtIndex(2);
25904 
25905         final var randomizer = new UniformRandomizer();
25906         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
25907         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25908         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25909         final var nedPosition = new NEDPosition(latitude, longitude, height);
25910         final var nedVelocity = new NEDVelocity();
25911         final var ecefPosition = new ECEFPosition();
25912         final var ecefVelocity = new ECEFVelocity();
25913         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
25914         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25915                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25916         final var gravityNorm = gravity.getNormAsAcceleration();
25917 
25918         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba);
25919 
25920         // check default values
25921         assertEquals(biasX, calibrator.getBiasX(), 0.0);
25922         assertEquals(biasY, calibrator.getBiasY(), 0.0);
25923         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
25924         final var bx1 = calibrator.getBiasXAsAcceleration();
25925         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
25926         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
25927         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25928         calibrator.getBiasXAsAcceleration(bx2);
25929         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
25930         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
25931         final var by1 = calibrator.getBiasYAsAcceleration();
25932         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
25933         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
25934         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25935         calibrator.getBiasYAsAcceleration(by2);
25936         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
25937         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
25938         final var bz1 = calibrator.getBiasZAsAcceleration();
25939         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
25940         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
25941         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
25942         calibrator.getBiasZAsAcceleration(bz2);
25943         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
25944         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
25945         final var biasTriad1 = calibrator.getBiasAsTriad();
25946         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
25947         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
25948         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
25949         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
25950         final var biasTriad2 = new AccelerationTriad();
25951         calibrator.getBiasAsTriad(biasTriad2);
25952         assertEquals(biasTriad1, biasTriad2);
25953         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
25954         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
25955         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
25956         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
25957         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
25958         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
25959         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
25960         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
25961         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
25962         final var bias1 = calibrator.getBias();
25963         assertArrayEquals(bias1, bias, 0.0);
25964         final var bias2 = new double[3];
25965         calibrator.getBias(bias2);
25966         assertArrayEquals(bias1, bias2, 0.0);
25967         final var b1 = calibrator.getBiasAsMatrix();
25968         assertEquals(b1, ba);
25969         final var b2 = new Matrix(3, 1);
25970         calibrator.getBiasAsMatrix(b2);
25971         assertEquals(b1, b2);
25972         final var ma1 = calibrator.getInitialMa();
25973         assertEquals(new Matrix(3, 3), ma1);
25974         final var ma2 = new Matrix(3, 3);
25975         calibrator.getInitialMa(ma2);
25976         assertEquals(ma1, ma2);
25977         assertNull(calibrator.getMeasurements());
25978         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
25979                 calibrator.getMeasurementType());
25980         assertFalse(calibrator.isOrderedMeasurementsRequired());
25981         assertFalse(calibrator.isQualityScoresRequired());
25982         assertFalse(calibrator.isCommonAxisUsed());
25983         assertNull(calibrator.getListener());
25984         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
25985         assertFalse(calibrator.isReady());
25986         assertFalse(calibrator.isRunning());
25987         assertNull(calibrator.getEstimatedMa());
25988         assertNull(calibrator.getEstimatedSx());
25989         assertNull(calibrator.getEstimatedSy());
25990         assertNull(calibrator.getEstimatedSz());
25991         assertNull(calibrator.getEstimatedMxy());
25992         assertNull(calibrator.getEstimatedMxz());
25993         assertNull(calibrator.getEstimatedMyx());
25994         assertNull(calibrator.getEstimatedMyz());
25995         assertNull(calibrator.getEstimatedMzx());
25996         assertNull(calibrator.getEstimatedMzy());
25997         assertNull(calibrator.getEstimatedCovariance());
25998         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
25999         assertNotNull(calibrator.getGroundTruthGravityNorm());
26000         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26001         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26002         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26003                 ABSOLUTE_ERROR));
26004         final var g = new Acceleration(0.0, AccelerationUnit.G);
26005         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26006         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26007         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26008 
26009         // Force IllegalArgumentException
26010         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26011 
26012         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26013                 invalidGravityNorm, ba));
26014         final var m1 = new Matrix(1, 1);
26015         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26016                 gravityNorm, m1));
26017         final var m2 = new Matrix(1, 3);
26018         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26019                 gravityNorm, m2));
26020     }
26021 
26022     @Test
26023     void testConstructor220() throws WrongSizeException {
26024         final var ba = generateBa();
26025         final var bias = ba.getBuffer();
26026         final var biasX = ba.getElementAtIndex(0);
26027         final var biasY = ba.getElementAtIndex(1);
26028         final var biasZ = ba.getElementAtIndex(2);
26029 
26030         final var randomizer = new UniformRandomizer();
26031         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26032         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26033         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26034         final var nedPosition = new NEDPosition(latitude, longitude, height);
26035         final var nedVelocity = new NEDVelocity();
26036         final var ecefPosition = new ECEFPosition();
26037         final var ecefVelocity = new ECEFVelocity();
26038         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26039         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26040                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26041         final var gravityNorm = gravity.getNormAsAcceleration();
26042 
26043         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, this);
26044 
26045         // check default values
26046         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26047         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26048         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26049         final var bx1 = calibrator.getBiasXAsAcceleration();
26050         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26051         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26052         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26053         calibrator.getBiasXAsAcceleration(bx2);
26054         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26055         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26056         final var by1 = calibrator.getBiasYAsAcceleration();
26057         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26058         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26059         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26060         calibrator.getBiasYAsAcceleration(by2);
26061         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26062         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26063         final var bz1 = calibrator.getBiasZAsAcceleration();
26064         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26065         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26066         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26067         calibrator.getBiasZAsAcceleration(bz2);
26068         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26069         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26070         final var biasTriad1 = calibrator.getBiasAsTriad();
26071         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26072         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26073         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26074         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26075         final var biasTriad2 = new AccelerationTriad();
26076         calibrator.getBiasAsTriad(biasTriad2);
26077         assertEquals(biasTriad1, biasTriad2);
26078         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26079         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26080         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26081         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26082         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26083         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26084         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26085         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26086         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26087         final var bias1 = calibrator.getBias();
26088         assertArrayEquals(bias1, bias, 0.0);
26089         final var bias2 = new double[3];
26090         calibrator.getBias(bias2);
26091         assertArrayEquals(bias1, bias2, 0.0);
26092         final var b1 = calibrator.getBiasAsMatrix();
26093         assertEquals(b1, ba);
26094         final var b2 = new Matrix(3, 1);
26095         calibrator.getBiasAsMatrix(b2);
26096         assertEquals(b1, b2);
26097         final var ma1 = calibrator.getInitialMa();
26098         assertEquals(new Matrix(3, 3), ma1);
26099         final var ma2 = new Matrix(3, 3);
26100         calibrator.getInitialMa(ma2);
26101         assertEquals(ma1, ma2);
26102         assertNull(calibrator.getMeasurements());
26103         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26104                 calibrator.getMeasurementType());
26105         assertFalse(calibrator.isOrderedMeasurementsRequired());
26106         assertFalse(calibrator.isQualityScoresRequired());
26107         assertFalse(calibrator.isCommonAxisUsed());
26108         assertSame(this, calibrator.getListener());
26109         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
26110         assertFalse(calibrator.isReady());
26111         assertFalse(calibrator.isRunning());
26112         assertNull(calibrator.getEstimatedMa());
26113         assertNull(calibrator.getEstimatedSx());
26114         assertNull(calibrator.getEstimatedSy());
26115         assertNull(calibrator.getEstimatedSz());
26116         assertNull(calibrator.getEstimatedMxy());
26117         assertNull(calibrator.getEstimatedMxz());
26118         assertNull(calibrator.getEstimatedMyx());
26119         assertNull(calibrator.getEstimatedMyz());
26120         assertNull(calibrator.getEstimatedMzx());
26121         assertNull(calibrator.getEstimatedMzy());
26122         assertNull(calibrator.getEstimatedCovariance());
26123         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26124         assertNotNull(calibrator.getGroundTruthGravityNorm());
26125         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26126         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26127         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26128                 ABSOLUTE_ERROR));
26129         final var g = new Acceleration(0.0, AccelerationUnit.G);
26130         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26131         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26132         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26133 
26134         // Force IllegalArgumentException
26135         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26136 
26137         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26138                 invalidGravityNorm, ba));
26139         final var m1 = new Matrix(1, 1);
26140         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26141                 gravityNorm, m1, this));
26142         final var m2 = new Matrix(1, 3);
26143         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26144                 gravityNorm, m2, this));
26145     }
26146 
26147     @Test
26148     void testConstructor221() throws WrongSizeException {
26149         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26150 
26151         final var ba = generateBa();
26152         final var bias = ba.getBuffer();
26153         final var biasX = ba.getElementAtIndex(0);
26154         final var biasY = ba.getElementAtIndex(1);
26155         final var biasZ = ba.getElementAtIndex(2);
26156 
26157         final var randomizer = new UniformRandomizer();
26158         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26159         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26160         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26161         final var nedPosition = new NEDPosition(latitude, longitude, height);
26162         final var nedVelocity = new NEDVelocity();
26163         final var ecefPosition = new ECEFPosition();
26164         final var ecefVelocity = new ECEFVelocity();
26165         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26166         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26167                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26168         final var gravityNorm = gravity.getNormAsAcceleration();
26169 
26170         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba);
26171 
26172         // check default values
26173         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26174         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26175         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26176         final var bx1 = calibrator.getBiasXAsAcceleration();
26177         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26178         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26179         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26180         calibrator.getBiasXAsAcceleration(bx2);
26181         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26182         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26183         final var by1 = calibrator.getBiasYAsAcceleration();
26184         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26185         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26186         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26187         calibrator.getBiasYAsAcceleration(by2);
26188         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26189         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26190         final var bz1 = calibrator.getBiasZAsAcceleration();
26191         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26192         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26193         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26194         calibrator.getBiasZAsAcceleration(bz2);
26195         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26196         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26197         final var biasTriad1 = calibrator.getBiasAsTriad();
26198         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26199         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26200         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26201         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26202         final var biasTriad2 = new AccelerationTriad();
26203         calibrator.getBiasAsTriad(biasTriad2);
26204         assertEquals(biasTriad1, biasTriad2);
26205         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26206         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26207         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26208         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26209         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26210         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26211         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26212         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26213         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26214         final var bias1 = calibrator.getBias();
26215         assertArrayEquals(bias1, bias, 0.0);
26216         final var bias2 = new double[3];
26217         calibrator.getBias(bias2);
26218         assertArrayEquals(bias1, bias2, 0.0);
26219         final var b1 = calibrator.getBiasAsMatrix();
26220         assertEquals(b1, ba);
26221         final var b2 = new Matrix(3, 1);
26222         calibrator.getBiasAsMatrix(b2);
26223         assertEquals(b1, b2);
26224         final var ma1 = calibrator.getInitialMa();
26225         assertEquals(new Matrix(3, 3), ma1);
26226         final var ma2 = new Matrix(3, 3);
26227         calibrator.getInitialMa(ma2);
26228         assertEquals(ma1, ma2);
26229         assertSame(measurements, calibrator.getMeasurements());
26230         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26231                 calibrator.getMeasurementType());
26232         assertFalse(calibrator.isOrderedMeasurementsRequired());
26233         assertFalse(calibrator.isQualityScoresRequired());
26234         assertFalse(calibrator.isCommonAxisUsed());
26235         assertNull(calibrator.getListener());
26236         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
26237         assertFalse(calibrator.isReady());
26238         assertFalse(calibrator.isRunning());
26239         assertNull(calibrator.getEstimatedMa());
26240         assertNull(calibrator.getEstimatedSx());
26241         assertNull(calibrator.getEstimatedSy());
26242         assertNull(calibrator.getEstimatedSz());
26243         assertNull(calibrator.getEstimatedMxy());
26244         assertNull(calibrator.getEstimatedMxz());
26245         assertNull(calibrator.getEstimatedMyx());
26246         assertNull(calibrator.getEstimatedMyz());
26247         assertNull(calibrator.getEstimatedMzx());
26248         assertNull(calibrator.getEstimatedMzy());
26249         assertNull(calibrator.getEstimatedCovariance());
26250         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26251         assertNotNull(calibrator.getGroundTruthGravityNorm());
26252         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26253         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26254         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26255                 ABSOLUTE_ERROR));
26256         final var g = new Acceleration(0.0, AccelerationUnit.G);
26257         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26258         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26259         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26260 
26261         // Force IllegalArgumentException
26262         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26263 
26264         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26265                 invalidGravityNorm, measurements, ba));
26266         final var m1 = new Matrix(1, 1);
26267         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26268                 gravityNorm, measurements, m1));
26269         final var m2 = new Matrix(1, 3);
26270         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26271                 gravityNorm, measurements, m2));
26272     }
26273 
26274     @Test
26275     void testConstructor222() throws WrongSizeException {
26276         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26277 
26278         final var ba = generateBa();
26279         final var bias = ba.getBuffer();
26280         final var biasX = ba.getElementAtIndex(0);
26281         final var biasY = ba.getElementAtIndex(1);
26282         final var biasZ = ba.getElementAtIndex(2);
26283 
26284         final var randomizer = new UniformRandomizer();
26285         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26286         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26287         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26288         final var nedPosition = new NEDPosition(latitude, longitude, height);
26289         final var nedVelocity = new NEDVelocity();
26290         final var ecefPosition = new ECEFPosition();
26291         final var ecefVelocity = new ECEFVelocity();
26292         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26293         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26294                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26295         final var gravityNorm = gravity.getNormAsAcceleration();
26296 
26297         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba,
26298                 this);
26299 
26300         // check default values
26301         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26302         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26303         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26304         final var bx1 = calibrator.getBiasXAsAcceleration();
26305         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26306         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26307         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26308         calibrator.getBiasXAsAcceleration(bx2);
26309         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26310         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26311         final var by1 = calibrator.getBiasYAsAcceleration();
26312         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26313         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26314         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26315         calibrator.getBiasYAsAcceleration(by2);
26316         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26317         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26318         final var bz1 = calibrator.getBiasZAsAcceleration();
26319         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26320         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26321         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26322         calibrator.getBiasZAsAcceleration(bz2);
26323         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26324         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26325         final var biasTriad1 = calibrator.getBiasAsTriad();
26326         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26327         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26328         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26329         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26330         final var biasTriad2 = new AccelerationTriad();
26331         calibrator.getBiasAsTriad(biasTriad2);
26332         assertEquals(biasTriad1, biasTriad2);
26333         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26334         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26335         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26336         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26337         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26338         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26339         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26340         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26341         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26342         final var bias1 = calibrator.getBias();
26343         assertArrayEquals(bias1, bias, 0.0);
26344         final var bias2 = new double[3];
26345         calibrator.getBias(bias2);
26346         assertArrayEquals(bias1, bias2, 0.0);
26347         final var b1 = calibrator.getBiasAsMatrix();
26348         assertEquals(b1, ba);
26349         final var b2 = new Matrix(3, 1);
26350         calibrator.getBiasAsMatrix(b2);
26351         assertEquals(b1, b2);
26352         final var ma1 = calibrator.getInitialMa();
26353         assertEquals(new Matrix(3, 3), ma1);
26354         final var ma2 = new Matrix(3, 3);
26355         calibrator.getInitialMa(ma2);
26356         assertEquals(ma1, ma2);
26357         assertSame(measurements, calibrator.getMeasurements());
26358         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26359                 calibrator.getMeasurementType());
26360         assertFalse(calibrator.isOrderedMeasurementsRequired());
26361         assertFalse(calibrator.isQualityScoresRequired());
26362         assertFalse(calibrator.isCommonAxisUsed());
26363         assertSame(this, calibrator.getListener());
26364         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
26365         assertFalse(calibrator.isReady());
26366         assertFalse(calibrator.isRunning());
26367         assertNull(calibrator.getEstimatedMa());
26368         assertNull(calibrator.getEstimatedSx());
26369         assertNull(calibrator.getEstimatedSy());
26370         assertNull(calibrator.getEstimatedSz());
26371         assertNull(calibrator.getEstimatedMxy());
26372         assertNull(calibrator.getEstimatedMxz());
26373         assertNull(calibrator.getEstimatedMyx());
26374         assertNull(calibrator.getEstimatedMyz());
26375         assertNull(calibrator.getEstimatedMzx());
26376         assertNull(calibrator.getEstimatedMzy());
26377         assertNull(calibrator.getEstimatedCovariance());
26378         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26379         assertNotNull(calibrator.getGroundTruthGravityNorm());
26380         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26381         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26382         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26383                 ABSOLUTE_ERROR));
26384         final var g = new Acceleration(0.0, AccelerationUnit.G);
26385         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26386         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26387         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26388 
26389         // Force IllegalArgumentException
26390         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26391 
26392         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26393                 invalidGravityNorm, measurements, ba, this));
26394         final var m1 = new Matrix(1, 1);
26395         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26396                 gravityNorm, measurements, m1, this));
26397         final var m2 = new Matrix(1, 3);
26398         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26399                 gravityNorm, measurements, m2, this));
26400     }
26401 
26402     @Test
26403     void testConstructor223() throws WrongSizeException {
26404         final var ba = generateBa();
26405         final var bias = ba.getBuffer();
26406         final var biasX = ba.getElementAtIndex(0);
26407         final var biasY = ba.getElementAtIndex(1);
26408         final var biasZ = ba.getElementAtIndex(2);
26409 
26410         final var randomizer = new UniformRandomizer();
26411         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26412         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26413         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26414         final var nedPosition = new NEDPosition(latitude, longitude, height);
26415         final var nedVelocity = new NEDVelocity();
26416         final var ecefPosition = new ECEFPosition();
26417         final var ecefVelocity = new ECEFVelocity();
26418         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26419         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26420                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26421         final var gravityNorm = gravity.getNormAsAcceleration();
26422 
26423         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba);
26424 
26425         // check default values
26426         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26427         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26428         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26429         final var bx1 = calibrator.getBiasXAsAcceleration();
26430         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26431         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26432         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26433         calibrator.getBiasXAsAcceleration(bx2);
26434         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26435         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26436         final var by1 = calibrator.getBiasYAsAcceleration();
26437         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26438         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26439         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26440         calibrator.getBiasYAsAcceleration(by2);
26441         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26442         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26443         final var bz1 = calibrator.getBiasZAsAcceleration();
26444         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26445         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26446         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26447         calibrator.getBiasZAsAcceleration(bz2);
26448         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26449         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26450         final var biasTriad1 = calibrator.getBiasAsTriad();
26451         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26452         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26453         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26454         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26455         final var biasTriad2 = new AccelerationTriad();
26456         calibrator.getBiasAsTriad(biasTriad2);
26457         assertEquals(biasTriad1, biasTriad2);
26458         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26459         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26460         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26461         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26462         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26463         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26464         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26465         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26466         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26467         final var bias1 = calibrator.getBias();
26468         assertArrayEquals(bias1, bias, 0.0);
26469         final var bias2 = new double[3];
26470         calibrator.getBias(bias2);
26471         assertArrayEquals(bias1, bias2, 0.0);
26472         final var b1 = calibrator.getBiasAsMatrix();
26473         assertEquals(b1, ba);
26474         final var b2 = new Matrix(3, 1);
26475         calibrator.getBiasAsMatrix(b2);
26476         assertEquals(b1, b2);
26477         final var ma1 = calibrator.getInitialMa();
26478         assertEquals(new Matrix(3, 3), ma1);
26479         final var ma2 = new Matrix(3, 3);
26480         calibrator.getInitialMa(ma2);
26481         assertEquals(ma1, ma2);
26482         assertNull(calibrator.getMeasurements());
26483         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26484                 calibrator.getMeasurementType());
26485         assertFalse(calibrator.isOrderedMeasurementsRequired());
26486         assertFalse(calibrator.isQualityScoresRequired());
26487         assertTrue(calibrator.isCommonAxisUsed());
26488         assertNull(calibrator.getListener());
26489         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26490         assertFalse(calibrator.isReady());
26491         assertFalse(calibrator.isRunning());
26492         assertNull(calibrator.getEstimatedMa());
26493         assertNull(calibrator.getEstimatedSx());
26494         assertNull(calibrator.getEstimatedSy());
26495         assertNull(calibrator.getEstimatedSz());
26496         assertNull(calibrator.getEstimatedMxy());
26497         assertNull(calibrator.getEstimatedMxz());
26498         assertNull(calibrator.getEstimatedMyx());
26499         assertNull(calibrator.getEstimatedMyz());
26500         assertNull(calibrator.getEstimatedMzx());
26501         assertNull(calibrator.getEstimatedMzy());
26502         assertNull(calibrator.getEstimatedCovariance());
26503         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26504         assertNotNull(calibrator.getGroundTruthGravityNorm());
26505         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26506         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26507         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26508                 ABSOLUTE_ERROR));
26509         final var g = new Acceleration(0.0, AccelerationUnit.G);
26510         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26511         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26512         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26513 
26514         // Force IllegalArgumentException
26515         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26516 
26517         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26518                 invalidGravityNorm, true, ba));
26519         final var m1 = new Matrix(1, 1);
26520         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26521                 gravityNorm, true, m1));
26522         final var m2 = new Matrix(1, 3);
26523         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26524                 gravityNorm, true, m2));
26525     }
26526 
26527     @Test
26528     void testConstructor224() throws WrongSizeException {
26529         final var ba = generateBa();
26530         final var bias = ba.getBuffer();
26531         final var biasX = ba.getElementAtIndex(0);
26532         final var biasY = ba.getElementAtIndex(1);
26533         final var biasZ = ba.getElementAtIndex(2);
26534 
26535         final var randomizer = new UniformRandomizer();
26536         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26537         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26538         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26539         final var nedPosition = new NEDPosition(latitude, longitude, height);
26540         final var nedVelocity = new NEDVelocity();
26541         final var ecefPosition = new ECEFPosition();
26542         final var ecefVelocity = new ECEFVelocity();
26543         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26544         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26545                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26546         final var gravityNorm = gravity.getNormAsAcceleration();
26547 
26548         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
26549                 this);
26550 
26551         // check default values
26552         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26553         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26554         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26555         final var bx1 = calibrator.getBiasXAsAcceleration();
26556         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26557         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26558         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26559         calibrator.getBiasXAsAcceleration(bx2);
26560         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26561         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26562         final var by1 = calibrator.getBiasYAsAcceleration();
26563         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26564         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26565         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26566         calibrator.getBiasYAsAcceleration(by2);
26567         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26568         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26569         final var bz1 = calibrator.getBiasZAsAcceleration();
26570         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26571         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26572         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26573         calibrator.getBiasZAsAcceleration(bz2);
26574         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26575         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26576         final var biasTriad1 = calibrator.getBiasAsTriad();
26577         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26578         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26579         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26580         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26581         final var biasTriad2 = new AccelerationTriad();
26582         calibrator.getBiasAsTriad(biasTriad2);
26583         assertEquals(biasTriad1, biasTriad2);
26584         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26585         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26586         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26587         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26588         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26589         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26590         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26591         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26592         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26593         final var bias1 = calibrator.getBias();
26594         assertArrayEquals(bias1, bias, 0.0);
26595         final var bias2 = new double[3];
26596         calibrator.getBias(bias2);
26597         assertArrayEquals(bias1, bias2, 0.0);
26598         final var b1 = calibrator.getBiasAsMatrix();
26599         assertEquals(b1, ba);
26600         final var b2 = new Matrix(3, 1);
26601         calibrator.getBiasAsMatrix(b2);
26602         assertEquals(b1, b2);
26603         final var ma1 = calibrator.getInitialMa();
26604         assertEquals(new Matrix(3, 3), ma1);
26605         final var ma2 = new Matrix(3, 3);
26606         calibrator.getInitialMa(ma2);
26607         assertEquals(ma1, ma2);
26608         assertNull(calibrator.getMeasurements());
26609         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26610                 calibrator.getMeasurementType());
26611         assertFalse(calibrator.isOrderedMeasurementsRequired());
26612         assertFalse(calibrator.isQualityScoresRequired());
26613         assertTrue(calibrator.isCommonAxisUsed());
26614         assertSame(this, calibrator.getListener());
26615         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26616         assertFalse(calibrator.isReady());
26617         assertFalse(calibrator.isRunning());
26618         assertNull(calibrator.getEstimatedMa());
26619         assertNull(calibrator.getEstimatedSx());
26620         assertNull(calibrator.getEstimatedSy());
26621         assertNull(calibrator.getEstimatedSz());
26622         assertNull(calibrator.getEstimatedMxy());
26623         assertNull(calibrator.getEstimatedMxz());
26624         assertNull(calibrator.getEstimatedMyx());
26625         assertNull(calibrator.getEstimatedMyz());
26626         assertNull(calibrator.getEstimatedMzx());
26627         assertNull(calibrator.getEstimatedMzy());
26628         assertNull(calibrator.getEstimatedCovariance());
26629         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26630         assertNotNull(calibrator.getGroundTruthGravityNorm());
26631         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26632         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26633         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26634                 ABSOLUTE_ERROR));
26635         final var g = new Acceleration(0.0, AccelerationUnit.G);
26636         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26637         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26638         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26639 
26640         // Force IllegalArgumentException
26641         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26642 
26643         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26644                 invalidGravityNorm, true, ba, this));
26645         final var m1 = new Matrix(1, 1);
26646         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26647                 gravityNorm, true, m1, this));
26648         final var m2 = new Matrix(1, 3);
26649         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26650                 gravityNorm, true, m2, this));
26651     }
26652 
26653     @Test
26654     void testConstructor225() throws WrongSizeException {
26655         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26656 
26657         final var ba = generateBa();
26658         final var bias = ba.getBuffer();
26659         final var biasX = ba.getElementAtIndex(0);
26660         final var biasY = ba.getElementAtIndex(1);
26661         final var biasZ = ba.getElementAtIndex(2);
26662 
26663         final var randomizer = new UniformRandomizer();
26664         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26665         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26666         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26667         final var nedPosition = new NEDPosition(latitude, longitude, height);
26668         final var nedVelocity = new NEDVelocity();
26669         final var ecefPosition = new ECEFPosition();
26670         final var ecefVelocity = new ECEFVelocity();
26671         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26672         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26673                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26674         final var gravityNorm = gravity.getNormAsAcceleration();
26675 
26676         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
26677                 true, ba);
26678 
26679         // check default values
26680         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26681         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26682         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26683         final var bx1 = calibrator.getBiasXAsAcceleration();
26684         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26685         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26686         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26687         calibrator.getBiasXAsAcceleration(bx2);
26688         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26689         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26690         final var by1 = calibrator.getBiasYAsAcceleration();
26691         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26692         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26693         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26694         calibrator.getBiasYAsAcceleration(by2);
26695         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26696         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26697         final var bz1 = calibrator.getBiasZAsAcceleration();
26698         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26699         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26700         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26701         calibrator.getBiasZAsAcceleration(bz2);
26702         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26703         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26704         final var biasTriad1 = calibrator.getBiasAsTriad();
26705         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26706         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26707         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26708         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26709         final var biasTriad2 = new AccelerationTriad();
26710         calibrator.getBiasAsTriad(biasTriad2);
26711         assertEquals(biasTriad1, biasTriad2);
26712         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26713         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26714         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26715         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26716         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26717         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26718         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26719         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26720         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26721         final var bias1 = calibrator.getBias();
26722         assertArrayEquals(bias1, bias, 0.0);
26723         final var bias2 = new double[3];
26724         calibrator.getBias(bias2);
26725         assertArrayEquals(bias1, bias2, 0.0);
26726         final var b1 = calibrator.getBiasAsMatrix();
26727         assertEquals(b1, ba);
26728         final var b2 = new Matrix(3, 1);
26729         calibrator.getBiasAsMatrix(b2);
26730         assertEquals(b1, b2);
26731         final var ma1 = calibrator.getInitialMa();
26732         assertEquals(new Matrix(3, 3), ma1);
26733         final var ma2 = new Matrix(3, 3);
26734         calibrator.getInitialMa(ma2);
26735         assertEquals(ma1, ma2);
26736         assertSame(measurements, calibrator.getMeasurements());
26737         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26738                 calibrator.getMeasurementType());
26739         assertFalse(calibrator.isOrderedMeasurementsRequired());
26740         assertFalse(calibrator.isQualityScoresRequired());
26741         assertTrue(calibrator.isCommonAxisUsed());
26742         assertNull(calibrator.getListener());
26743         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26744         assertFalse(calibrator.isReady());
26745         assertFalse(calibrator.isRunning());
26746         assertNull(calibrator.getEstimatedMa());
26747         assertNull(calibrator.getEstimatedSx());
26748         assertNull(calibrator.getEstimatedSy());
26749         assertNull(calibrator.getEstimatedSz());
26750         assertNull(calibrator.getEstimatedMxy());
26751         assertNull(calibrator.getEstimatedMxz());
26752         assertNull(calibrator.getEstimatedMyx());
26753         assertNull(calibrator.getEstimatedMyz());
26754         assertNull(calibrator.getEstimatedMzx());
26755         assertNull(calibrator.getEstimatedMzy());
26756         assertNull(calibrator.getEstimatedCovariance());
26757         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26758         assertNotNull(calibrator.getGroundTruthGravityNorm());
26759         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26760         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26761         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26762                 ABSOLUTE_ERROR));
26763         final var g = new Acceleration(0.0, AccelerationUnit.G);
26764         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26765         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26766         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26767 
26768         // Force IllegalArgumentException
26769         final var invalidGravityNorm = new Acceleration(
26770                 -gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26771 
26772         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26773                 invalidGravityNorm, measurements, true, ba));
26774         final var m1 = new Matrix(1, 1);
26775         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26776                 gravityNorm, measurements, true, m1));
26777         final var m2 = new Matrix(1, 3);
26778         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26779                 gravityNorm, measurements, true, m2));
26780     }
26781 
26782     @Test
26783     void testConstructor226() throws WrongSizeException {
26784         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
26785 
26786         final var ba = generateBa();
26787         final var bias = ba.getBuffer();
26788         final var biasX = ba.getElementAtIndex(0);
26789         final var biasY = ba.getElementAtIndex(1);
26790         final var biasZ = ba.getElementAtIndex(2);
26791 
26792         final var randomizer = new UniformRandomizer();
26793         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26794         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26795         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26796         final var nedPosition = new NEDPosition(latitude, longitude, height);
26797         final var nedVelocity = new NEDVelocity();
26798         final var ecefPosition = new ECEFPosition();
26799         final var ecefVelocity = new ECEFVelocity();
26800         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26801         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26802                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26803         final var gravityNorm = gravity.getNormAsAcceleration();
26804 
26805         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
26806                 true, ba, this);
26807 
26808         // check default values
26809         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26810         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26811         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26812         final var bx1 = calibrator.getBiasXAsAcceleration();
26813         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26814         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26815         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26816         calibrator.getBiasXAsAcceleration(bx2);
26817         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26818         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26819         final var by1 = calibrator.getBiasYAsAcceleration();
26820         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26821         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26822         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26823         calibrator.getBiasYAsAcceleration(by2);
26824         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26825         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26826         final var bz1 = calibrator.getBiasZAsAcceleration();
26827         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26828         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26829         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26830         calibrator.getBiasZAsAcceleration(bz2);
26831         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26832         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26833         final var biasTriad1 = calibrator.getBiasAsTriad();
26834         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26835         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26836         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26837         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26838         final var biasTriad2 = new AccelerationTriad();
26839         calibrator.getBiasAsTriad(biasTriad2);
26840         assertEquals(biasTriad1, biasTriad2);
26841         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
26842         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
26843         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
26844         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
26845         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
26846         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
26847         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
26848         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
26849         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
26850         final var bias1 = calibrator.getBias();
26851         assertArrayEquals(bias1, bias, 0.0);
26852         final var bias2 = new double[3];
26853         calibrator.getBias(bias2);
26854         assertArrayEquals(bias1, bias2, 0.0);
26855         final var b1 = calibrator.getBiasAsMatrix();
26856         assertEquals(b1, ba);
26857         final var b2 = new Matrix(3, 1);
26858         calibrator.getBiasAsMatrix(b2);
26859         assertEquals(b1, b2);
26860         final var ma1 = calibrator.getInitialMa();
26861         assertEquals(new Matrix(3, 3), ma1);
26862         final var ma2 = new Matrix(3, 3);
26863         calibrator.getInitialMa(ma2);
26864         assertEquals(ma1, ma2);
26865         assertSame(measurements, calibrator.getMeasurements());
26866         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
26867                 calibrator.getMeasurementType());
26868         assertFalse(calibrator.isOrderedMeasurementsRequired());
26869         assertFalse(calibrator.isQualityScoresRequired());
26870         assertTrue(calibrator.isCommonAxisUsed());
26871         assertSame(this, calibrator.getListener());
26872         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
26873         assertFalse(calibrator.isReady());
26874         assertFalse(calibrator.isRunning());
26875         assertNull(calibrator.getEstimatedMa());
26876         assertNull(calibrator.getEstimatedSx());
26877         assertNull(calibrator.getEstimatedSy());
26878         assertNull(calibrator.getEstimatedSz());
26879         assertNull(calibrator.getEstimatedMxy());
26880         assertNull(calibrator.getEstimatedMxz());
26881         assertNull(calibrator.getEstimatedMyx());
26882         assertNull(calibrator.getEstimatedMyz());
26883         assertNull(calibrator.getEstimatedMzx());
26884         assertNull(calibrator.getEstimatedMzy());
26885         assertNull(calibrator.getEstimatedCovariance());
26886         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
26887         assertNotNull(calibrator.getGroundTruthGravityNorm());
26888         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26889         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26890         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
26891                 ABSOLUTE_ERROR));
26892         final var g = new Acceleration(0.0, AccelerationUnit.G);
26893         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26894         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26895         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
26896 
26897         // Force IllegalArgumentException
26898         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26899 
26900         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26901                 invalidGravityNorm, measurements, true, ba, this));
26902         final var m1 = new Matrix(1, 1);
26903         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26904                 gravityNorm, measurements, true, m1, this));
26905         final var m2 = new Matrix(1, 3);
26906         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
26907                 gravityNorm, measurements, true, m2, this));
26908     }
26909 
26910     @Test
26911     void testConstructor227() throws WrongSizeException {
26912         final var ba = generateBa();
26913         final var bias = ba.getBuffer();
26914         final var biasX = ba.getElementAtIndex(0);
26915         final var biasY = ba.getElementAtIndex(1);
26916         final var biasZ = ba.getElementAtIndex(2);
26917 
26918         final var ma = generateMaCommonAxis();
26919         final var sx = ma.getElementAt(0, 0);
26920         final var sy = ma.getElementAt(1, 1);
26921         final var sz = ma.getElementAt(2, 2);
26922         final var mxy = ma.getElementAt(0, 1);
26923         final var mxz = ma.getElementAt(0, 2);
26924         final var myx = ma.getElementAt(1, 0);
26925         final var myz = ma.getElementAt(1, 2);
26926         final var mzx = ma.getElementAt(2, 0);
26927         final var mzy = ma.getElementAt(2, 1);
26928 
26929         final var randomizer = new UniformRandomizer();
26930         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
26931         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26932         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26933         final var nedPosition = new NEDPosition(latitude, longitude, height);
26934         final var nedVelocity = new NEDVelocity();
26935         final var ecefPosition = new ECEFPosition();
26936         final var ecefVelocity = new ECEFVelocity();
26937         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
26938         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26939                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26940         final var gravityNorm = gravity.getNormAsAcceleration();
26941 
26942         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma);
26943 
26944         // check default values
26945         assertEquals(biasX, calibrator.getBiasX(), 0.0);
26946         assertEquals(biasY, calibrator.getBiasY(), 0.0);
26947         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
26948         final var bx1 = calibrator.getBiasXAsAcceleration();
26949         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
26950         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
26951         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26952         calibrator.getBiasXAsAcceleration(bx2);
26953         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
26954         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
26955         final var by1 = calibrator.getBiasYAsAcceleration();
26956         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
26957         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
26958         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26959         calibrator.getBiasYAsAcceleration(by2);
26960         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
26961         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
26962         final var bz1 = calibrator.getBiasZAsAcceleration();
26963         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
26964         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
26965         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
26966         calibrator.getBiasZAsAcceleration(bz2);
26967         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
26968         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
26969         final var biasTriad1 = calibrator.getBiasAsTriad();
26970         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
26971         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
26972         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
26973         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
26974         final var biasTriad2 = new AccelerationTriad();
26975         calibrator.getBiasAsTriad(biasTriad2);
26976         assertEquals(biasTriad1, biasTriad2);
26977         assertEquals(sx, calibrator.getInitialSx(), 0.0);
26978         assertEquals(sy, calibrator.getInitialSy(), 0.0);
26979         assertEquals(sz, calibrator.getInitialSz(), 0.0);
26980         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
26981         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
26982         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
26983         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
26984         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
26985         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
26986         final var bias1 = calibrator.getBias();
26987         assertArrayEquals(bias1, bias, 0.0);
26988         final var bias2 = new double[3];
26989         calibrator.getBias(bias2);
26990         assertArrayEquals(bias1, bias2, 0.0);
26991         final var b1 = calibrator.getBiasAsMatrix();
26992         assertEquals(b1, ba);
26993         final var b2 = new Matrix(3, 1);
26994         calibrator.getBiasAsMatrix(b2);
26995         assertEquals(b1, b2);
26996         final var ma1 = new Matrix(3, 3);
26997         ma1.setSubmatrix(0, 0, 2, 2,
26998                 new double[]{sx, myx, mzx,
26999                         mxy, sy, mzy,
27000                         mxz, myz, sz});
27001         assertEquals(ma1, calibrator.getInitialMa());
27002         final var ma2 = new Matrix(3, 3);
27003         calibrator.getInitialMa(ma2);
27004         assertEquals(ma1, ma2);
27005         assertNull(calibrator.getMeasurements());
27006         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27007                 calibrator.getMeasurementType());
27008         assertFalse(calibrator.isOrderedMeasurementsRequired());
27009         assertFalse(calibrator.isQualityScoresRequired());
27010         assertFalse(calibrator.isCommonAxisUsed());
27011         assertNull(calibrator.getListener());
27012         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27013         assertFalse(calibrator.isReady());
27014         assertFalse(calibrator.isRunning());
27015         assertNull(calibrator.getEstimatedMa());
27016         assertNull(calibrator.getEstimatedSx());
27017         assertNull(calibrator.getEstimatedSy());
27018         assertNull(calibrator.getEstimatedSz());
27019         assertNull(calibrator.getEstimatedMxy());
27020         assertNull(calibrator.getEstimatedMxz());
27021         assertNull(calibrator.getEstimatedMyx());
27022         assertNull(calibrator.getEstimatedMyz());
27023         assertNull(calibrator.getEstimatedMzx());
27024         assertNull(calibrator.getEstimatedMzy());
27025         assertNull(calibrator.getEstimatedCovariance());
27026         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27027         assertNotNull(calibrator.getGroundTruthGravityNorm());
27028         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27029         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27030         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27031                 ABSOLUTE_ERROR));
27032         final var g = new Acceleration(0.0, AccelerationUnit.G);
27033         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27034         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27035         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27036 
27037         // Force IllegalArgumentException
27038         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27039 
27040         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27041                 invalidGravityNorm, ba, ma));
27042         final var m1 = new Matrix(1, 1);
27043         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27044                 gravityNorm, m1, ma));
27045         final var m2 = new Matrix(1, 3);
27046         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27047                 gravityNorm, m2, ma));
27048         final var m3 = new Matrix(1, 3);
27049         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27050                 gravityNorm, ba, m3));
27051         final var m4 = new Matrix(3, 1);
27052         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27053                 gravityNorm, ba, m4));
27054     }
27055 
27056     @Test
27057     void testConstructor228() throws WrongSizeException {
27058         final var ba = generateBa();
27059         final var bias = ba.getBuffer();
27060         final var biasX = ba.getElementAtIndex(0);
27061         final var biasY = ba.getElementAtIndex(1);
27062         final var biasZ = ba.getElementAtIndex(2);
27063 
27064         final var ma = generateMaCommonAxis();
27065         final var sx = ma.getElementAt(0, 0);
27066         final var sy = ma.getElementAt(1, 1);
27067         final var sz = ma.getElementAt(2, 2);
27068         final var mxy = ma.getElementAt(0, 1);
27069         final var mxz = ma.getElementAt(0, 2);
27070         final var myx = ma.getElementAt(1, 0);
27071         final var myz = ma.getElementAt(1, 2);
27072         final var mzx = ma.getElementAt(2, 0);
27073         final var mzy = ma.getElementAt(2, 1);
27074 
27075         final var randomizer = new UniformRandomizer();
27076         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27077         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27078         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27079         final var nedPosition = new NEDPosition(latitude, longitude, height);
27080         final var nedVelocity = new NEDVelocity();
27081         final var ecefPosition = new ECEFPosition();
27082         final var ecefVelocity = new ECEFVelocity();
27083         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27084         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27085                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27086         final var gravityNorm = gravity.getNormAsAcceleration();
27087 
27088         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, ba, ma, this);
27089 
27090         // check default values
27091         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27092         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27093         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27094         final var bx1 = calibrator.getBiasXAsAcceleration();
27095         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27096         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27097         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27098         calibrator.getBiasXAsAcceleration(bx2);
27099         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27100         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27101         final var by1 = calibrator.getBiasYAsAcceleration();
27102         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27103         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27104         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27105         calibrator.getBiasYAsAcceleration(by2);
27106         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27107         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27108         final var bz1 = calibrator.getBiasZAsAcceleration();
27109         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27110         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27111         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27112         calibrator.getBiasZAsAcceleration(bz2);
27113         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27114         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27115         final var biasTriad1 = calibrator.getBiasAsTriad();
27116         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27117         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27118         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27119         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27120         final var biasTriad2 = new AccelerationTriad();
27121         calibrator.getBiasAsTriad(biasTriad2);
27122         assertEquals(biasTriad1, biasTriad2);
27123         assertEquals(sx, calibrator.getInitialSx(), 0.0);
27124         assertEquals(sy, calibrator.getInitialSy(), 0.0);
27125         assertEquals(sz, calibrator.getInitialSz(), 0.0);
27126         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27127         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27128         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27129         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27130         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27131         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27132         final var bias1 = calibrator.getBias();
27133         assertArrayEquals(bias1, bias, 0.0);
27134         final var bias2 = new double[3];
27135         calibrator.getBias(bias2);
27136         assertArrayEquals(bias1, bias2, 0.0);
27137         final var b1 = calibrator.getBiasAsMatrix();
27138         assertEquals(b1, ba);
27139         final var b2 = new Matrix(3, 1);
27140         calibrator.getBiasAsMatrix(b2);
27141         assertEquals(b1, b2);
27142         final var ma1 = new Matrix(3, 3);
27143         ma1.setSubmatrix(0, 0, 2, 2,
27144                 new double[]{sx, myx, mzx,
27145                         mxy, sy, mzy,
27146                         mxz, myz, sz});
27147         assertEquals(ma1, calibrator.getInitialMa());
27148         final var ma2 = new Matrix(3, 3);
27149         calibrator.getInitialMa(ma2);
27150         assertEquals(ma1, ma2);
27151         assertNull(calibrator.getMeasurements());
27152         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27153                 calibrator.getMeasurementType());
27154         assertFalse(calibrator.isOrderedMeasurementsRequired());
27155         assertFalse(calibrator.isQualityScoresRequired());
27156         assertFalse(calibrator.isCommonAxisUsed());
27157         assertSame(this, calibrator.getListener());
27158         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27159         assertFalse(calibrator.isReady());
27160         assertFalse(calibrator.isRunning());
27161         assertNull(calibrator.getEstimatedMa());
27162         assertNull(calibrator.getEstimatedSx());
27163         assertNull(calibrator.getEstimatedSy());
27164         assertNull(calibrator.getEstimatedSz());
27165         assertNull(calibrator.getEstimatedMxy());
27166         assertNull(calibrator.getEstimatedMxz());
27167         assertNull(calibrator.getEstimatedMyx());
27168         assertNull(calibrator.getEstimatedMyz());
27169         assertNull(calibrator.getEstimatedMzx());
27170         assertNull(calibrator.getEstimatedMzy());
27171         assertNull(calibrator.getEstimatedCovariance());
27172         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27173         assertNotNull(calibrator.getGroundTruthGravityNorm());
27174         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27175         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27176         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27177                 ABSOLUTE_ERROR));
27178         final var g = new Acceleration(0.0, AccelerationUnit.G);
27179         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27180         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27181         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27182 
27183         // Force IllegalArgumentException
27184         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27185 
27186         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27187                 invalidGravityNorm, ba, ma, this));
27188         final var m1 = new Matrix(1, 1);
27189         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27190                 gravityNorm, m1, ma, this));
27191         final var m2 = new Matrix(1, 3);
27192         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27193                 gravityNorm, m2, ma, this));
27194         final var m3 = new Matrix(1, 3);
27195         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27196                 gravityNorm, ba, m3, this));
27197         final var m4 = new Matrix(3, 1);
27198         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27199                 gravityNorm, ba, m4, this));
27200     }
27201 
27202     @Test
27203     void testConstructor229() throws WrongSizeException {
27204         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27205 
27206         final var ba = generateBa();
27207         final var bias = ba.getBuffer();
27208         final var biasX = ba.getElementAtIndex(0);
27209         final var biasY = ba.getElementAtIndex(1);
27210         final var biasZ = ba.getElementAtIndex(2);
27211 
27212         final var ma = generateMaCommonAxis();
27213         final var sx = ma.getElementAt(0, 0);
27214         final var sy = ma.getElementAt(1, 1);
27215         final var sz = ma.getElementAt(2, 2);
27216         final var mxy = ma.getElementAt(0, 1);
27217         final var mxz = ma.getElementAt(0, 2);
27218         final var myx = ma.getElementAt(1, 0);
27219         final var myz = ma.getElementAt(1, 2);
27220         final var mzx = ma.getElementAt(2, 0);
27221         final var mzy = ma.getElementAt(2, 1);
27222 
27223         final var randomizer = new UniformRandomizer();
27224         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27225         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27226         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27227         final var nedPosition = new NEDPosition(latitude, longitude, height);
27228         final var nedVelocity = new NEDVelocity();
27229         final var ecefPosition = new ECEFPosition();
27230         final var ecefVelocity = new ECEFVelocity();
27231         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27232         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27233                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27234         final var gravityNorm = gravity.getNormAsAcceleration();
27235 
27236         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma);
27237 
27238         // check default values
27239         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27240         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27241         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27242         final var bx1 = calibrator.getBiasXAsAcceleration();
27243         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27244         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27245         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27246         calibrator.getBiasXAsAcceleration(bx2);
27247         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27248         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27249         final var by1 = calibrator.getBiasYAsAcceleration();
27250         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27251         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27252         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27253         calibrator.getBiasYAsAcceleration(by2);
27254         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27255         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27256         final var bz1 = calibrator.getBiasZAsAcceleration();
27257         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27258         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27259         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27260         calibrator.getBiasZAsAcceleration(bz2);
27261         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27262         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27263         final var biasTriad1 = calibrator.getBiasAsTriad();
27264         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27265         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27266         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27267         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27268         final var biasTriad2 = new AccelerationTriad();
27269         calibrator.getBiasAsTriad(biasTriad2);
27270         assertEquals(biasTriad1, biasTriad2);
27271         assertEquals(sx, calibrator.getInitialSx(), 0.0);
27272         assertEquals(sy, calibrator.getInitialSy(), 0.0);
27273         assertEquals(sz, calibrator.getInitialSz(), 0.0);
27274         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27275         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27276         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27277         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27278         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27279         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27280         final var bias1 = calibrator.getBias();
27281         assertArrayEquals(bias1, bias, 0.0);
27282         final var bias2 = new double[3];
27283         calibrator.getBias(bias2);
27284         assertArrayEquals(bias1, bias2, 0.0);
27285         final var b1 = calibrator.getBiasAsMatrix();
27286         assertEquals(b1, ba);
27287         final var b2 = new Matrix(3, 1);
27288         calibrator.getBiasAsMatrix(b2);
27289         assertEquals(b1, b2);
27290         final var ma1 = new Matrix(3, 3);
27291         ma1.setSubmatrix(0, 0, 2, 2,
27292                 new double[]{sx, myx, mzx,
27293                         mxy, sy, mzy,
27294                         mxz, myz, sz});
27295         assertEquals(ma1, calibrator.getInitialMa());
27296         final var ma2 = new Matrix(3, 3);
27297         calibrator.getInitialMa(ma2);
27298         assertEquals(ma1, ma2);
27299         assertSame(measurements, calibrator.getMeasurements());
27300         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27301                 calibrator.getMeasurementType());
27302         assertFalse(calibrator.isOrderedMeasurementsRequired());
27303         assertFalse(calibrator.isQualityScoresRequired());
27304         assertFalse(calibrator.isCommonAxisUsed());
27305         assertNull(calibrator.getListener());
27306         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27307         assertFalse(calibrator.isReady());
27308         assertFalse(calibrator.isRunning());
27309         assertNull(calibrator.getEstimatedMa());
27310         assertNull(calibrator.getEstimatedSx());
27311         assertNull(calibrator.getEstimatedSy());
27312         assertNull(calibrator.getEstimatedSz());
27313         assertNull(calibrator.getEstimatedMxy());
27314         assertNull(calibrator.getEstimatedMxz());
27315         assertNull(calibrator.getEstimatedMyx());
27316         assertNull(calibrator.getEstimatedMyz());
27317         assertNull(calibrator.getEstimatedMzx());
27318         assertNull(calibrator.getEstimatedMzy());
27319         assertNull(calibrator.getEstimatedCovariance());
27320         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27321         assertNotNull(calibrator.getGroundTruthGravityNorm());
27322         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27323         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27324         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27325                 ABSOLUTE_ERROR));
27326         final var g = new Acceleration(0.0, AccelerationUnit.G);
27327         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27328         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27329         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27330 
27331         // Force IllegalArgumentException
27332         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27333 
27334         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27335                 invalidGravityNorm, ba, ma, this));
27336         final var m1 = new Matrix(1, 1);
27337         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27338                 gravityNorm, measurements, m1, ma));
27339         final var m2 = new Matrix(1, 3);
27340         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27341                 gravityNorm, measurements, m2, ma));
27342         final var m3 = new Matrix(1, 3);
27343         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27344                 gravityNorm, measurements, ba, m3));
27345         final var m4 = new Matrix(3, 1);
27346         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27347                 gravityNorm, measurements, ba, m4));
27348     }
27349 
27350     @Test
27351     void testConstructor230() throws WrongSizeException {
27352         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27353 
27354         final var ba = generateBa();
27355         final var bias = ba.getBuffer();
27356         final var biasX = ba.getElementAtIndex(0);
27357         final var biasY = ba.getElementAtIndex(1);
27358         final var biasZ = ba.getElementAtIndex(2);
27359 
27360         final var ma = generateMaCommonAxis();
27361         final var sx = ma.getElementAt(0, 0);
27362         final var sy = ma.getElementAt(1, 1);
27363         final var sz = ma.getElementAt(2, 2);
27364         final var mxy = ma.getElementAt(0, 1);
27365         final var mxz = ma.getElementAt(0, 2);
27366         final var myx = ma.getElementAt(1, 0);
27367         final var myz = ma.getElementAt(1, 2);
27368         final var mzx = ma.getElementAt(2, 0);
27369         final var mzy = ma.getElementAt(2, 1);
27370 
27371         final var randomizer = new UniformRandomizer();
27372         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27373         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27374         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27375         final var nedPosition = new NEDPosition(latitude, longitude, height);
27376         final var nedVelocity = new NEDVelocity();
27377         final var ecefPosition = new ECEFPosition();
27378         final var ecefVelocity = new ECEFVelocity();
27379         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27380         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27381                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27382         final var gravityNorm = gravity.getNormAsAcceleration();
27383 
27384         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements, ba, ma,
27385                 this);
27386 
27387         // check default values
27388         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27389         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27390         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27391         final var bx1 = calibrator.getBiasXAsAcceleration();
27392         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27393         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27394         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27395         calibrator.getBiasXAsAcceleration(bx2);
27396         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27397         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27398         final var by1 = calibrator.getBiasYAsAcceleration();
27399         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27400         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27401         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27402         calibrator.getBiasYAsAcceleration(by2);
27403         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27404         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27405         final var bz1 = calibrator.getBiasZAsAcceleration();
27406         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27407         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27408         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27409         calibrator.getBiasZAsAcceleration(bz2);
27410         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27411         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27412         final var biasTriad1 = calibrator.getBiasAsTriad();
27413         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27414         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27415         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27416         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27417         final var biasTriad2 = new AccelerationTriad();
27418         calibrator.getBiasAsTriad(biasTriad2);
27419         assertEquals(biasTriad1, biasTriad2);
27420         assertEquals(sx, calibrator.getInitialSx(), 0.0);
27421         assertEquals(sy, calibrator.getInitialSy(), 0.0);
27422         assertEquals(sz, calibrator.getInitialSz(), 0.0);
27423         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27424         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27425         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27426         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27427         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27428         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27429         final var bias1 = calibrator.getBias();
27430         assertArrayEquals(bias1, bias, 0.0);
27431         final var bias2 = new double[3];
27432         calibrator.getBias(bias2);
27433         assertArrayEquals(bias1, bias2, 0.0);
27434         final var b1 = calibrator.getBiasAsMatrix();
27435         assertEquals(b1, ba);
27436         final var b2 = new Matrix(3, 1);
27437         calibrator.getBiasAsMatrix(b2);
27438         assertEquals(b1, b2);
27439         final var ma1 = new Matrix(3, 3);
27440         ma1.setSubmatrix(0, 0, 2, 2,
27441                 new double[]{sx, myx, mzx,
27442                         mxy, sy, mzy,
27443                         mxz, myz, sz});
27444         assertEquals(calibrator.getInitialMa(), ma1);
27445         final var ma2 = new Matrix(3, 3);
27446         calibrator.getInitialMa(ma2);
27447         assertEquals(ma1, ma2);
27448         assertSame(measurements, calibrator.getMeasurements());
27449         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27450                 calibrator.getMeasurementType());
27451         assertFalse(calibrator.isOrderedMeasurementsRequired());
27452         assertFalse(calibrator.isQualityScoresRequired());
27453         assertFalse(calibrator.isCommonAxisUsed());
27454         assertSame(this, calibrator.getListener());
27455         assertEquals(10, calibrator.getMinimumRequiredMeasurements());
27456         assertFalse(calibrator.isReady());
27457         assertFalse(calibrator.isRunning());
27458         assertNull(calibrator.getEstimatedMa());
27459         assertNull(calibrator.getEstimatedSx());
27460         assertNull(calibrator.getEstimatedSy());
27461         assertNull(calibrator.getEstimatedSz());
27462         assertNull(calibrator.getEstimatedMxy());
27463         assertNull(calibrator.getEstimatedMxz());
27464         assertNull(calibrator.getEstimatedMyx());
27465         assertNull(calibrator.getEstimatedMyz());
27466         assertNull(calibrator.getEstimatedMzx());
27467         assertNull(calibrator.getEstimatedMzy());
27468         assertNull(calibrator.getEstimatedCovariance());
27469         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27470         assertNotNull(calibrator.getGroundTruthGravityNorm());
27471         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27472         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27473         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27474                 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27475         final var g = new Acceleration(0.0, AccelerationUnit.G);
27476         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27477         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27478         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27479 
27480         // Force IllegalArgumentException
27481         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27482 
27483         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27484                 invalidGravityNorm, ba, ma, this));
27485         final var m1 = new Matrix(1, 1);
27486         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27487                 gravityNorm, measurements, m1, ma, this));
27488         final var m2 = new Matrix(1, 3);
27489         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27490                 gravityNorm, measurements, m2, ma, this));
27491         final var m3 = new Matrix(1, 3);
27492         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27493                 gravityNorm, measurements, ba, m3, this));
27494         final var m4 = new Matrix(3, 1);
27495         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27496                 gravityNorm, measurements, ba, m4, this));
27497     }
27498 
27499     @Test
27500     void testConstructor231() throws WrongSizeException {
27501         final var ba = generateBa();
27502         final var bias = ba.getBuffer();
27503         final var biasX = ba.getElementAtIndex(0);
27504         final var biasY = ba.getElementAtIndex(1);
27505         final var biasZ = ba.getElementAtIndex(2);
27506 
27507         final var ma = generateMaCommonAxis();
27508         final var sx = ma.getElementAt(0, 0);
27509         final var sy = ma.getElementAt(1, 1);
27510         final var sz = ma.getElementAt(2, 2);
27511         final var mxy = ma.getElementAt(0, 1);
27512         final var mxz = ma.getElementAt(0, 2);
27513         final var myx = ma.getElementAt(1, 0);
27514         final var myz = ma.getElementAt(1, 2);
27515         final var mzx = ma.getElementAt(2, 0);
27516         final var mzy = ma.getElementAt(2, 1);
27517 
27518         final var randomizer = new UniformRandomizer();
27519         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27520         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27521         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27522         final var nedPosition = new NEDPosition(latitude, longitude, height);
27523         final var nedVelocity = new NEDVelocity();
27524         final var ecefPosition = new ECEFPosition();
27525         final var ecefVelocity = new ECEFVelocity();
27526         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27527         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27528                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27529         final var gravityNorm = gravity.getNormAsAcceleration();
27530 
27531         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
27532                 ma);
27533 
27534         // check default values
27535         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27536         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27537         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27538         final var bx1 = calibrator.getBiasXAsAcceleration();
27539         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27540         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27541         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27542         calibrator.getBiasXAsAcceleration(bx2);
27543         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27544         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27545         final var by1 = calibrator.getBiasYAsAcceleration();
27546         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27547         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27548         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27549         calibrator.getBiasYAsAcceleration(by2);
27550         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27551         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27552         final var bz1 = calibrator.getBiasZAsAcceleration();
27553         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27554         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27555         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27556         calibrator.getBiasZAsAcceleration(bz2);
27557         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27558         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27559         final var biasTriad1 = calibrator.getBiasAsTriad();
27560         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27561         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27562         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27563         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27564         final var biasTriad2 = new AccelerationTriad();
27565         calibrator.getBiasAsTriad(biasTriad2);
27566         assertEquals(biasTriad1, biasTriad2);
27567         assertEquals(sx, calibrator.getInitialSx(), 0.0);
27568         assertEquals(sy, calibrator.getInitialSy(), 0.0);
27569         assertEquals(sz, calibrator.getInitialSz(), 0.0);
27570         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27571         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27572         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27573         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27574         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27575         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27576         final var bias1 = calibrator.getBias();
27577         assertArrayEquals(bias1, bias, 0.0);
27578         final var bias2 = new double[3];
27579         calibrator.getBias(bias2);
27580         assertArrayEquals(bias1, bias2, 0.0);
27581         final var b1 = calibrator.getBiasAsMatrix();
27582         assertEquals(b1, ba);
27583         final var b2 = new Matrix(3, 1);
27584         calibrator.getBiasAsMatrix(b2);
27585         assertEquals(b1, b2);
27586         final var ma1 = new Matrix(3, 3);
27587         ma1.setSubmatrix(0, 0, 2, 2,
27588                 new double[]{sx, myx, mzx,
27589                         mxy, sy, mzy,
27590                         mxz, myz, sz});
27591         assertEquals(ma1, calibrator.getInitialMa());
27592         final var ma2 = new Matrix(3, 3);
27593         calibrator.getInitialMa(ma2);
27594         assertEquals(ma1, ma2);
27595         assertNull(calibrator.getMeasurements());
27596         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27597                 calibrator.getMeasurementType());
27598         assertFalse(calibrator.isOrderedMeasurementsRequired());
27599         assertFalse(calibrator.isQualityScoresRequired());
27600         assertTrue(calibrator.isCommonAxisUsed());
27601         assertNull(calibrator.getListener());
27602         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
27603         assertFalse(calibrator.isReady());
27604         assertFalse(calibrator.isRunning());
27605         assertNull(calibrator.getEstimatedMa());
27606         assertNull(calibrator.getEstimatedSx());
27607         assertNull(calibrator.getEstimatedSy());
27608         assertNull(calibrator.getEstimatedSz());
27609         assertNull(calibrator.getEstimatedMxy());
27610         assertNull(calibrator.getEstimatedMxz());
27611         assertNull(calibrator.getEstimatedMyx());
27612         assertNull(calibrator.getEstimatedMyz());
27613         assertNull(calibrator.getEstimatedMzx());
27614         assertNull(calibrator.getEstimatedMzy());
27615         assertNull(calibrator.getEstimatedCovariance());
27616         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27617         assertNotNull(calibrator.getGroundTruthGravityNorm());
27618         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27619         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27620         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27621                 ABSOLUTE_ERROR));
27622         final var g = new Acceleration(0.0, AccelerationUnit.G);
27623         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27624         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27625         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27626 
27627         // Force IllegalArgumentException
27628         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27629 
27630         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27631                 invalidGravityNorm, true, ba, ma));
27632         final var m1 = new Matrix(1, 1);
27633         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27634                 gravityNorm, true, m1, ma));
27635         final var m2 = new Matrix(1, 3);
27636         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27637                 gravityNorm, true, m2, ma));
27638         final var m3 = new Matrix(1, 3);
27639         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27640                 gravityNorm, true, ba, m3));
27641         final var m4 = new Matrix(3, 1);
27642         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27643                 gravityNorm, true, ba, m4));
27644     }
27645 
27646     @Test
27647     void testConstructor232() throws WrongSizeException {
27648         final var ba = generateBa();
27649         final var bias = ba.getBuffer();
27650         final var biasX = ba.getElementAtIndex(0);
27651         final var biasY = ba.getElementAtIndex(1);
27652         final var biasZ = ba.getElementAtIndex(2);
27653 
27654         final var ma = generateMaCommonAxis();
27655         final var sx = ma.getElementAt(0, 0);
27656         final var sy = ma.getElementAt(1, 1);
27657         final var sz = ma.getElementAt(2, 2);
27658         final var mxy = ma.getElementAt(0, 1);
27659         final var mxz = ma.getElementAt(0, 2);
27660         final var myx = ma.getElementAt(1, 0);
27661         final var myz = ma.getElementAt(1, 2);
27662         final var mzx = ma.getElementAt(2, 0);
27663         final var mzy = ma.getElementAt(2, 1);
27664 
27665         final var randomizer = new UniformRandomizer();
27666         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27667         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27668         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27669         final var nedPosition = new NEDPosition(latitude, longitude, height);
27670         final var nedVelocity = new NEDVelocity();
27671         final var ecefPosition = new ECEFPosition();
27672         final var ecefVelocity = new ECEFVelocity();
27673         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27674         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27675                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27676         final var gravityNorm = gravity.getNormAsAcceleration();
27677 
27678         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, true, ba,
27679                 ma, this);
27680 
27681         // check default values
27682         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27683         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27684         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27685         final var bx1 = calibrator.getBiasXAsAcceleration();
27686         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27687         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27688         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27689         calibrator.getBiasXAsAcceleration(bx2);
27690         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27691         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27692         final var by1 = calibrator.getBiasYAsAcceleration();
27693         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27694         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27695         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27696         calibrator.getBiasYAsAcceleration(by2);
27697         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27698         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27699         final var bz1 = calibrator.getBiasZAsAcceleration();
27700         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27701         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27702         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27703         calibrator.getBiasZAsAcceleration(bz2);
27704         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27705         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27706         final var biasTriad1 = calibrator.getBiasAsTriad();
27707         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27708         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27709         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27710         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27711         final var biasTriad2 = new AccelerationTriad();
27712         calibrator.getBiasAsTriad(biasTriad2);
27713         assertEquals(biasTriad1, biasTriad2);
27714         assertEquals(sx, calibrator.getInitialSx(), 0.0);
27715         assertEquals(sy, calibrator.getInitialSy(), 0.0);
27716         assertEquals(sz, calibrator.getInitialSz(), 0.0);
27717         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27718         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27719         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27720         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27721         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27722         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27723         final var bias1 = calibrator.getBias();
27724         assertArrayEquals(bias1, bias, 0.0);
27725         final var bias2 = new double[3];
27726         calibrator.getBias(bias2);
27727         assertArrayEquals(bias1, bias2, 0.0);
27728         final var b1 = calibrator.getBiasAsMatrix();
27729         assertEquals(b1, ba);
27730         final var b2 = new Matrix(3, 1);
27731         calibrator.getBiasAsMatrix(b2);
27732         assertEquals(b1, b2);
27733         final var ma1 = new Matrix(3, 3);
27734         ma1.setSubmatrix(0, 0, 2, 2,
27735                 new double[]{sx, myx, mzx,
27736                         mxy, sy, mzy,
27737                         mxz, myz, sz});
27738         assertEquals(ma1, calibrator.getInitialMa());
27739         final var ma2 = new Matrix(3, 3);
27740         calibrator.getInitialMa(ma2);
27741         assertEquals(ma1, ma2);
27742         assertNull(calibrator.getMeasurements());
27743         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27744                 calibrator.getMeasurementType());
27745         assertFalse(calibrator.isOrderedMeasurementsRequired());
27746         assertFalse(calibrator.isQualityScoresRequired());
27747         assertTrue(calibrator.isCommonAxisUsed());
27748         assertSame(this, calibrator.getListener());
27749         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
27750         assertFalse(calibrator.isReady());
27751         assertFalse(calibrator.isRunning());
27752         assertNull(calibrator.getEstimatedMa());
27753         assertNull(calibrator.getEstimatedSx());
27754         assertNull(calibrator.getEstimatedSy());
27755         assertNull(calibrator.getEstimatedSz());
27756         assertNull(calibrator.getEstimatedMxy());
27757         assertNull(calibrator.getEstimatedMxz());
27758         assertNull(calibrator.getEstimatedMyx());
27759         assertNull(calibrator.getEstimatedMyz());
27760         assertNull(calibrator.getEstimatedMzx());
27761         assertNull(calibrator.getEstimatedMzy());
27762         assertNull(calibrator.getEstimatedCovariance());
27763         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27764         assertNotNull(calibrator.getGroundTruthGravityNorm());
27765         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27766         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27767         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27768                 ABSOLUTE_ERROR));
27769         final var g = new Acceleration(0.0, AccelerationUnit.G);
27770         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27771         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27772         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27773 
27774         // Force IllegalArgumentException
27775         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27776 
27777         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27778                 invalidGravityNorm, true, ba, ma));
27779         final var m1 = new Matrix(1, 1);
27780         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27781                 gravityNorm, true, m1, ma, this));
27782         final var m2 = new Matrix(1, 3);
27783         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27784                 gravityNorm, true, m2, ma, this));
27785         final var m3 = new Matrix(1, 3);
27786         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27787                 gravityNorm, true, ba, m3, this));
27788         final var m4 = new Matrix(3, 1);
27789         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27790                 gravityNorm, true, ba, m4, this));
27791     }
27792 
27793     @Test
27794     void testConstructor233() throws WrongSizeException {
27795         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27796 
27797         final var ba = generateBa();
27798         final var bias = ba.getBuffer();
27799         final var biasX = ba.getElementAtIndex(0);
27800         final var biasY = ba.getElementAtIndex(1);
27801         final var biasZ = ba.getElementAtIndex(2);
27802 
27803         final var ma = generateMaCommonAxis();
27804         final var sx = ma.getElementAt(0, 0);
27805         final var sy = ma.getElementAt(1, 1);
27806         final var sz = ma.getElementAt(2, 2);
27807         final var mxy = ma.getElementAt(0, 1);
27808         final var mxz = ma.getElementAt(0, 2);
27809         final var myx = ma.getElementAt(1, 0);
27810         final var myz = ma.getElementAt(1, 2);
27811         final var mzx = ma.getElementAt(2, 0);
27812         final var mzy = ma.getElementAt(2, 1);
27813 
27814         final var randomizer = new UniformRandomizer();
27815         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27816         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27817         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27818         final var nedPosition = new NEDPosition(latitude, longitude, height);
27819         final var nedVelocity = new NEDVelocity();
27820         final var ecefPosition = new ECEFPosition();
27821         final var ecefVelocity = new ECEFVelocity();
27822         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27823         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27824                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27825         final var gravityNorm = gravity.getNormAsAcceleration();
27826 
27827         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
27828                 true, ba, ma);
27829 
27830         // check default values
27831         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27832         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27833         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27834         final var bx1 = calibrator.getBiasXAsAcceleration();
27835         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27836         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27837         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27838         calibrator.getBiasXAsAcceleration(bx2);
27839         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27840         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27841         final var by1 = calibrator.getBiasYAsAcceleration();
27842         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27843         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27844         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27845         calibrator.getBiasYAsAcceleration(by2);
27846         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27847         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27848         final var bz1 = calibrator.getBiasZAsAcceleration();
27849         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27850         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
27851         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27852         calibrator.getBiasZAsAcceleration(bz2);
27853         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
27854         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
27855         final var biasTriad1 = calibrator.getBiasAsTriad();
27856         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
27857         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
27858         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
27859         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
27860         final var biasTriad2 = new AccelerationTriad();
27861         calibrator.getBiasAsTriad(biasTriad2);
27862         assertEquals(biasTriad1, biasTriad2);
27863         assertEquals(sx, calibrator.getInitialSx(), 0.0);
27864         assertEquals(sy, calibrator.getInitialSy(), 0.0);
27865         assertEquals(sz, calibrator.getInitialSz(), 0.0);
27866         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
27867         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
27868         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
27869         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
27870         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
27871         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
27872         final var bias1 = calibrator.getBias();
27873         assertArrayEquals(bias1, bias, 0.0);
27874         final var bias2 = new double[3];
27875         calibrator.getBias(bias2);
27876         assertArrayEquals(bias1, bias2, 0.0);
27877         final var b1 = calibrator.getBiasAsMatrix();
27878         assertEquals(b1, ba);
27879         final var b2 = new Matrix(3, 1);
27880         calibrator.getBiasAsMatrix(b2);
27881         assertEquals(b1, b2);
27882         final var ma1 = new Matrix(3, 3);
27883         ma1.setSubmatrix(0, 0, 2, 2,
27884                 new double[]{sx, myx, mzx,
27885                         mxy, sy, mzy,
27886                         mxz, myz, sz});
27887         assertEquals(ma1, calibrator.getInitialMa());
27888         final var ma2 = new Matrix(3, 3);
27889         calibrator.getInitialMa(ma2);
27890         assertEquals(ma1, ma2);
27891         assertSame(measurements, calibrator.getMeasurements());
27892         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
27893                 calibrator.getMeasurementType());
27894         assertFalse(calibrator.isOrderedMeasurementsRequired());
27895         assertFalse(calibrator.isQualityScoresRequired());
27896         assertTrue(calibrator.isCommonAxisUsed());
27897         assertNull(calibrator.getListener());
27898         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
27899         assertFalse(calibrator.isReady());
27900         assertFalse(calibrator.isRunning());
27901         assertNull(calibrator.getEstimatedMa());
27902         assertNull(calibrator.getEstimatedSx());
27903         assertNull(calibrator.getEstimatedSy());
27904         assertNull(calibrator.getEstimatedSz());
27905         assertNull(calibrator.getEstimatedMxy());
27906         assertNull(calibrator.getEstimatedMxz());
27907         assertNull(calibrator.getEstimatedMyx());
27908         assertNull(calibrator.getEstimatedMyz());
27909         assertNull(calibrator.getEstimatedMzx());
27910         assertNull(calibrator.getEstimatedMzy());
27911         assertNull(calibrator.getEstimatedCovariance());
27912         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
27913         assertNotNull(calibrator.getGroundTruthGravityNorm());
27914         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27915         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27916         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
27917                 ABSOLUTE_ERROR));
27918         final var g = new Acceleration(0.0, AccelerationUnit.G);
27919         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27920         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27921         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
27922 
27923         // Force IllegalArgumentException
27924         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27925 
27926         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27927                 invalidGravityNorm, measurements, true, ba, ma));
27928         final var m1 = new Matrix(1, 1);
27929         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27930                 gravityNorm, measurements, true, m1, ma));
27931         final var m2 = new Matrix(1, 3);
27932         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27933                 gravityNorm, measurements, true, m2, ma));
27934         final var m3 = new Matrix(1, 3);
27935         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27936                 gravityNorm, measurements, true, ba, m3));
27937         final var m4 = new Matrix(3, 1);
27938         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
27939                 gravityNorm, measurements, true, ba, m4));
27940     }
27941 
27942     @Test
27943     void testConstructor234() throws WrongSizeException {
27944         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
27945 
27946         final var ba = generateBa();
27947         final var bias = ba.getBuffer();
27948         final var biasX = ba.getElementAtIndex(0);
27949         final var biasY = ba.getElementAtIndex(1);
27950         final var biasZ = ba.getElementAtIndex(2);
27951 
27952         final var ma = generateMaCommonAxis();
27953         final var sx = ma.getElementAt(0, 0);
27954         final var sy = ma.getElementAt(1, 1);
27955         final var sz = ma.getElementAt(2, 2);
27956         final var mxy = ma.getElementAt(0, 1);
27957         final var mxz = ma.getElementAt(0, 2);
27958         final var myx = ma.getElementAt(1, 0);
27959         final var myz = ma.getElementAt(1, 2);
27960         final var mzx = ma.getElementAt(2, 0);
27961         final var mzy = ma.getElementAt(2, 1);
27962 
27963         final var randomizer = new UniformRandomizer();
27964         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
27965         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27966         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27967         final var nedPosition = new NEDPosition(latitude, longitude, height);
27968         final var nedVelocity = new NEDVelocity();
27969         final var ecefPosition = new ECEFPosition();
27970         final var ecefVelocity = new ECEFVelocity();
27971         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
27972         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27973                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27974         final var gravityNorm = gravity.getNormAsAcceleration();
27975 
27976         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravityNorm, measurements,
27977                 true, ba, ma, this);
27978 
27979         // check default values
27980         assertEquals(biasX, calibrator.getBiasX(), 0.0);
27981         assertEquals(biasY, calibrator.getBiasY(), 0.0);
27982         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
27983         final var bx1 = calibrator.getBiasXAsAcceleration();
27984         assertEquals(biasX, bx1.getValue().doubleValue(), 0.0);
27985         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
27986         final var bx2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27987         calibrator.getBiasXAsAcceleration(bx2);
27988         assertEquals(biasX, bx2.getValue().doubleValue(), 0.0);
27989         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx2.getUnit());
27990         final var by1 = calibrator.getBiasYAsAcceleration();
27991         assertEquals(biasY, by1.getValue().doubleValue(), 0.0);
27992         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
27993         final var by2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
27994         calibrator.getBiasYAsAcceleration(by2);
27995         assertEquals(biasY, by2.getValue().doubleValue(), 0.0);
27996         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by2.getUnit());
27997         final var bz1 = calibrator.getBiasZAsAcceleration();
27998         assertEquals(biasZ, bz1.getValue().doubleValue(), 0.0);
27999         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
28000         final var bz2 = new Acceleration(0.0, AccelerationUnit.FEET_PER_SQUARED_SECOND);
28001         calibrator.getBiasZAsAcceleration(bz2);
28002         assertEquals(biasZ, bz2.getValue().doubleValue(), 0.0);
28003         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz2.getUnit());
28004         final var biasTriad1 = calibrator.getBiasAsTriad();
28005         assertEquals(biasX, biasTriad1.getValueX(), 0.0);
28006         assertEquals(biasY, biasTriad1.getValueY(), 0.0);
28007         assertEquals(biasZ, biasTriad1.getValueZ(), 0.0);
28008         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasTriad1.getUnit());
28009         final var biasTriad2 = new AccelerationTriad();
28010         calibrator.getBiasAsTriad(biasTriad2);
28011         assertEquals(biasTriad1, biasTriad2);
28012         assertEquals(sx, calibrator.getInitialSx(), 0.0);
28013         assertEquals(sy, calibrator.getInitialSy(), 0.0);
28014         assertEquals(sz, calibrator.getInitialSz(), 0.0);
28015         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28016         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
28017         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28018         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28019         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28020         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28021         final var bias1 = calibrator.getBias();
28022         assertArrayEquals(bias1, bias, 0.0);
28023         final var bias2 = new double[3];
28024         calibrator.getBias(bias2);
28025         assertArrayEquals(bias1, bias2, 0.0);
28026         final var b1 = calibrator.getBiasAsMatrix();
28027         assertEquals(b1, ba);
28028         final var b2 = new Matrix(3, 1);
28029         calibrator.getBiasAsMatrix(b2);
28030         assertEquals(b1, b2);
28031         final var ma1 = new Matrix(3, 3);
28032         ma1.setSubmatrix(0, 0, 2, 2,
28033                 new double[]{sx, myx, mzx,
28034                         mxy, sy, mzy,
28035                         mxz, myz, sz});
28036         assertEquals(ma1, calibrator.getInitialMa());
28037         final var ma2 = new Matrix(3, 3);
28038         calibrator.getInitialMa(ma2);
28039         assertEquals(ma1, ma2);
28040         assertSame(measurements, calibrator.getMeasurements());
28041         assertEquals(AccelerometerCalibratorMeasurementType.STANDARD_DEVIATION_BODY_KINEMATICS,
28042                 calibrator.getMeasurementType());
28043         assertFalse(calibrator.isOrderedMeasurementsRequired());
28044         assertFalse(calibrator.isQualityScoresRequired());
28045         assertTrue(calibrator.isCommonAxisUsed());
28046         assertSame(this, calibrator.getListener());
28047         assertEquals(7, calibrator.getMinimumRequiredMeasurements());
28048         assertFalse(calibrator.isReady());
28049         assertFalse(calibrator.isRunning());
28050         assertNull(calibrator.getEstimatedMa());
28051         assertNull(calibrator.getEstimatedSx());
28052         assertNull(calibrator.getEstimatedSy());
28053         assertNull(calibrator.getEstimatedSz());
28054         assertNull(calibrator.getEstimatedMxy());
28055         assertNull(calibrator.getEstimatedMxz());
28056         assertNull(calibrator.getEstimatedMyx());
28057         assertNull(calibrator.getEstimatedMyz());
28058         assertNull(calibrator.getEstimatedMzx());
28059         assertNull(calibrator.getEstimatedMzy());
28060         assertNull(calibrator.getEstimatedCovariance());
28061         assertEquals(0.0, calibrator.getEstimatedChiSq(), 0.0);
28062         assertNotNull(calibrator.getGroundTruthGravityNorm());
28063         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28064         assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28065         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration().equals(gravity.getNormAsAcceleration(),
28066                 ABSOLUTE_ERROR));
28067         final var g = new Acceleration(0.0, AccelerationUnit.G);
28068         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28069         assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28070         assertEquals(0.0, calibrator.getEstimatedMse(), 0.0);
28071 
28072         // Force IllegalArgumentException
28073         final var invalidGravityNorm = new Acceleration(-gravity.getNorm(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28074 
28075         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28076                 invalidGravityNorm, measurements, true, ba, ma, this));
28077         final var m1 = new Matrix(1, 1);
28078         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28079                 gravityNorm, measurements, true, m1, ma, this));
28080         final var m2 = new Matrix(1, 3);
28081         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28082                 gravityNorm, measurements, true, m2, ma, this));
28083         final var m3 = new Matrix(1, 3);
28084         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28085                 gravityNorm, measurements, true, ba, m3, this));
28086         final var m4 = new Matrix(3, 1);
28087         assertThrows(IllegalArgumentException.class, () -> new KnownBiasAndGravityNormAccelerometerCalibrator(
28088                 gravityNorm, measurements, true, ba, m4, this));
28089     }
28090 
28091     @Test
28092     void testGetSetBiasX() throws LockedException {
28093         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28094 
28095         assertEquals(0.0, calibrator.getBiasX(), 0.0);
28096 
28097         // set new value
28098         final var ba = generateBa();
28099         final var biasX = ba.getElementAtIndex(0);
28100 
28101         calibrator.setBiasX(biasX);
28102 
28103         // check
28104         assertEquals(biasX, calibrator.getBiasX(), 0.0);
28105     }
28106 
28107     @Test
28108     void testGetSetBiasY() throws LockedException {
28109         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28110 
28111         assertEquals(0.0, calibrator.getBiasY(), 0.0);
28112 
28113         // set new value
28114         final var ba = generateBa();
28115         final var biasY = ba.getElementAtIndex(1);
28116 
28117         calibrator.setBiasY(biasY);
28118 
28119         // check
28120         assertEquals(biasY, calibrator.getBiasY(), 0.0);
28121     }
28122 
28123     @Test
28124     void testGetSetBiasZ() throws LockedException {
28125         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28126 
28127         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
28128 
28129         // set new value
28130         final var ba = generateBa();
28131         final var biasZ = ba.getElementAtIndex(2);
28132 
28133         calibrator.setBiasZ(biasZ);
28134 
28135         // check
28136         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
28137     }
28138 
28139     @Test
28140     void testGetSetBiasAsTriad() throws LockedException {
28141         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28142 
28143         // check default values
28144         final var triad1 = calibrator.getBiasAsTriad();
28145         assertEquals(0.0, triad1.getValueX(), 0.0);
28146         assertEquals(0.0, triad1.getValueY(), 0.0);
28147         assertEquals(0.0, triad1.getValueZ(), 0.0);
28148         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, triad1.getUnit());
28149 
28150         // set new values
28151         final var ba = generateBa();
28152         final var biasX = ba.getElementAtIndex(0);
28153         final var biasY = ba.getElementAtIndex(1);
28154         final var biasZ = ba.getElementAtIndex(2);
28155 
28156         final var triad2 = new AccelerationTriad(AccelerationUnit.METERS_PER_SQUARED_SECOND, biasX, biasY, biasZ);
28157         calibrator.setBias(triad2);
28158 
28159         // check
28160         final var triad3 = calibrator.getBiasAsTriad();
28161         final var triad4 = new AccelerationTriad();
28162         calibrator.getBiasAsTriad(triad4);
28163 
28164         assertEquals(triad2, triad3);
28165         assertEquals(triad2, triad4);
28166     }
28167 
28168     @Test
28169     void testGetSetBiasXAsAcceleration() throws LockedException {
28170         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28171 
28172         // check default value
28173         final var bx1 = calibrator.getBiasXAsAcceleration();
28174 
28175         assertEquals(0.0, bx1.getValue().doubleValue(), 0.0);
28176         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bx1.getUnit());
28177 
28178         // set new value
28179         final var ba = generateBa();
28180         final var biasX = ba.getElementAtIndex(0);
28181 
28182         final var bx2 = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28183         calibrator.setBiasX(bx2);
28184 
28185         // check
28186         final var bx3 = calibrator.getBiasXAsAcceleration();
28187         final var bx4 = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28188         calibrator.getBiasXAsAcceleration(bx4);
28189 
28190         assertEquals(bx3, bx4);
28191     }
28192 
28193     @Test
28194     void testGetSetBiasYAsAcceleration() throws LockedException {
28195         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28196 
28197         // check default value
28198         final var by1 = calibrator.getBiasYAsAcceleration();
28199 
28200         assertEquals(0.0, by1.getValue().doubleValue(), 0.0);
28201         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, by1.getUnit());
28202 
28203         // set new value
28204         final var ba = generateBa();
28205         final var biasY = ba.getElementAtIndex(1);
28206 
28207         final var by2 = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28208         calibrator.setBiasY(by2);
28209 
28210         // check
28211         final var by3 = calibrator.getBiasYAsAcceleration();
28212         final var by4 = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28213         calibrator.getBiasYAsAcceleration(by4);
28214 
28215         assertEquals(by3, by4);
28216     }
28217 
28218     @Test
28219     void testGetSetBiasZAsAcceleration() throws LockedException {
28220         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28221 
28222         // check default value
28223         final var bz1 = calibrator.getBiasZAsAcceleration();
28224 
28225         assertEquals(0.0, bz1.getValue().doubleValue(), 0.0);
28226         assertEquals(AccelerationUnit.METERS_PER_SQUARED_SECOND, bz1.getUnit());
28227 
28228         // set new value
28229         final var ba = generateBa();
28230         final var biasZ = ba.getElementAtIndex(2);
28231 
28232         final var bz2 = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28233         calibrator.setBiasZ(bz2);
28234 
28235         // check
28236         final var bz3 = calibrator.getBiasZAsAcceleration();
28237         final var bz4 = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28238         calibrator.getBiasZAsAcceleration(bz4);
28239 
28240         assertEquals(bz3, bz4);
28241     }
28242 
28243     @Test
28244     void testSetBiasCoordinates1() throws LockedException {
28245         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28246 
28247         // check default values
28248         assertEquals(0.0, calibrator.getBiasX(), 0.0);
28249         assertEquals(0.0, calibrator.getBiasY(), 0.0);
28250         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
28251 
28252         // set new values
28253         final var ba = generateBa();
28254         final var biasX = ba.getElementAtIndex(0);
28255         final var biasY = ba.getElementAtIndex(1);
28256         final var biasZ = ba.getElementAtIndex(2);
28257 
28258         calibrator.setBiasCoordinates(biasX, biasY, biasZ);
28259 
28260         // check
28261         assertEquals(biasX, calibrator.getBiasX(), 0.0);
28262         assertEquals(biasY, calibrator.getBiasY(), 0.0);
28263         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
28264     }
28265 
28266     @Test
28267     void testSetBias2() throws LockedException {
28268         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28269 
28270         // check default values
28271         assertEquals(0.0, calibrator.getBiasX(), 0.0);
28272         assertEquals(0.0, calibrator.getBiasY(), 0.0);
28273         assertEquals(0.0, calibrator.getBiasZ(), 0.0);
28274 
28275         // set new values
28276         final var ba = generateBa();
28277         final var biasX = ba.getElementAtIndex(0);
28278         final var biasY = ba.getElementAtIndex(1);
28279         final var biasZ = ba.getElementAtIndex(2);
28280 
28281         final var bx = new Acceleration(biasX, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28282         final var by = new Acceleration(biasY, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28283         final var bz = new Acceleration(biasZ, AccelerationUnit.METERS_PER_SQUARED_SECOND);
28284 
28285         calibrator.setBiasCoordinates(bx, by, bz);
28286 
28287         // check
28288         assertEquals(biasX, calibrator.getBiasX(), 0.0);
28289         assertEquals(biasY, calibrator.getBiasY(), 0.0);
28290         assertEquals(biasZ, calibrator.getBiasZ(), 0.0);
28291     }
28292 
28293     @Test
28294     void testGetSetInitialSx() throws WrongSizeException, LockedException {
28295         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28296 
28297         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
28298 
28299         // set new value
28300         final var ma = generateMaGeneral();
28301         final var sx = ma.getElementAt(0, 0);
28302 
28303         calibrator.setInitialSx(sx);
28304 
28305         // check
28306         assertEquals(sx, calibrator.getInitialSx(), 0.0);
28307     }
28308 
28309     @Test
28310     void testGetSetInitialSy() throws WrongSizeException, LockedException {
28311         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28312 
28313         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
28314 
28315         // set new value
28316         final var ma = generateMaGeneral();
28317         final var sy = ma.getElementAt(1, 1);
28318 
28319         calibrator.setInitialSy(sy);
28320 
28321         // check
28322         assertEquals(sy, calibrator.getInitialSy(), 0.0);
28323     }
28324 
28325     @Test
28326     void testGetSetInitialSz() throws WrongSizeException, LockedException {
28327         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28328 
28329         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
28330 
28331         // set new value
28332         final var ma = generateMaGeneral();
28333         final var sz = ma.getElementAt(2, 2);
28334 
28335         calibrator.setInitialSz(sz);
28336 
28337         // check
28338         assertEquals(sz, calibrator.getInitialSz(), 0.0);
28339     }
28340 
28341     @Test
28342     void testGetSetInitialMxy() throws WrongSizeException, LockedException {
28343         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28344 
28345         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
28346 
28347         // set new value
28348         final var ma = generateMaGeneral();
28349         final var mxy = ma.getElementAt(0, 1);
28350 
28351         calibrator.setInitialMxy(mxy);
28352 
28353         // check
28354         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28355     }
28356 
28357     @Test
28358     void testGetSetInitialMxz() throws WrongSizeException, LockedException {
28359         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28360 
28361         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
28362 
28363         // set new value
28364         final var ma = generateMaGeneral();
28365         final var mxz = ma.getElementAt(0, 2);
28366 
28367         calibrator.setInitialMxz(mxz);
28368 
28369         // check
28370         assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
28371     }
28372 
28373     @Test
28374     void testGetSetInitialMyx() throws WrongSizeException, LockedException {
28375         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28376 
28377         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
28378 
28379         // set new value
28380         final var ma = generateMaGeneral();
28381         final var myx = ma.getElementAt(1, 0);
28382 
28383         calibrator.setInitialMyx(myx);
28384 
28385         // check
28386         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28387     }
28388 
28389     @Test
28390     void testGetSetInitialMyz() throws WrongSizeException, LockedException {
28391         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28392 
28393         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
28394 
28395         // set new value
28396         final var ma = generateMaGeneral();
28397         final var myz = ma.getElementAt(1, 2);
28398 
28399         calibrator.setInitialMyz(myz);
28400 
28401         // check
28402         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28403     }
28404 
28405     @Test
28406     void testGetSetInitialMzx() throws WrongSizeException, LockedException {
28407         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28408 
28409         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
28410 
28411         // set new value
28412         final var ma = generateMaGeneral();
28413         final var mzx = ma.getElementAt(2, 0);
28414 
28415         calibrator.setInitialMzx(mzx);
28416 
28417         // check
28418         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28419     }
28420 
28421     @Test
28422     void testGetSetInitialMzy() throws WrongSizeException, LockedException {
28423         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28424 
28425         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
28426 
28427         // set new value
28428         final var ma = generateMaGeneral();
28429         final var mzy = ma.getElementAt(2, 1);
28430 
28431         calibrator.setInitialMzy(mzy);
28432 
28433         // check
28434         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28435     }
28436 
28437     @Test
28438     void testSetInitialScalingFactors() throws WrongSizeException, LockedException {
28439 
28440         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28441 
28442         // check default values
28443         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
28444         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
28445         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
28446 
28447         // set new values
28448         final var ma = generateMaGeneral();
28449         final var sx = ma.getElementAt(0, 0);
28450         final var sy = ma.getElementAt(1, 1);
28451         final var sz = ma.getElementAt(2, 2);
28452 
28453         calibrator.setInitialScalingFactors(sx, sy, sz);
28454 
28455         // check
28456         assertEquals(sx, calibrator.getInitialSx(), 0.0);
28457         assertEquals(sy, calibrator.getInitialSy(), 0.0);
28458         assertEquals(sz, calibrator.getInitialSz(), 0.0);
28459     }
28460 
28461     @Test
28462     void testSetInitialCrossCouplingErrors() throws WrongSizeException, LockedException {
28463 
28464         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28465 
28466         // check default values
28467         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
28468         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
28469         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
28470         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
28471         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
28472         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
28473 
28474         // set new values
28475         final var ma = generateMaGeneral();
28476         final var mxy = ma.getElementAt(0, 1);
28477         final var mxz = ma.getElementAt(0, 2);
28478         final var myx = ma.getElementAt(1, 0);
28479         final var myz = ma.getElementAt(1, 2);
28480         final var mzx = ma.getElementAt(2, 0);
28481         final var mzy = ma.getElementAt(2, 1);
28482 
28483         calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
28484 
28485         // check
28486         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28487         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
28488         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28489         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28490         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28491         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28492     }
28493 
28494     @Test
28495     void testSetInitialScalingFactorsAndCrossCouplingErrors() throws WrongSizeException, LockedException {
28496 
28497         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28498 
28499         // check default values
28500         assertEquals(0.0, calibrator.getInitialSx(), 0.0);
28501         assertEquals(0.0, calibrator.getInitialSy(), 0.0);
28502         assertEquals(0.0, calibrator.getInitialSz(), 0.0);
28503         assertEquals(0.0, calibrator.getInitialMxy(), 0.0);
28504         assertEquals(0.0, calibrator.getInitialMxz(), 0.0);
28505         assertEquals(0.0, calibrator.getInitialMyx(), 0.0);
28506         assertEquals(0.0, calibrator.getInitialMyz(), 0.0);
28507         assertEquals(0.0, calibrator.getInitialMzx(), 0.0);
28508         assertEquals(0.0, calibrator.getInitialMzy(), 0.0);
28509 
28510         // set new values
28511         final var ma = generateMaGeneral();
28512         final var sx = ma.getElementAt(0, 0);
28513         final var sy = ma.getElementAt(1, 1);
28514         final var sz = ma.getElementAt(2, 2);
28515         final var mxy = ma.getElementAt(0, 1);
28516         final var mxz = ma.getElementAt(0, 2);
28517         final var myx = ma.getElementAt(1, 0);
28518         final var myz = ma.getElementAt(1, 2);
28519         final var mzx = ma.getElementAt(2, 0);
28520         final var mzy = ma.getElementAt(2, 1);
28521 
28522         calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
28523 
28524         // check
28525         assertEquals(sx, calibrator.getInitialSx(), 0.0);
28526         assertEquals(sy, calibrator.getInitialSy(), 0.0);
28527         assertEquals(sz, calibrator.getInitialSz(), 0.0);
28528         assertEquals(mxy, calibrator.getInitialMxy(), 0.0);
28529         assertEquals(mxz, calibrator.getInitialMxz(), 0.0);
28530         assertEquals(myx, calibrator.getInitialMyx(), 0.0);
28531         assertEquals(myz, calibrator.getInitialMyz(), 0.0);
28532         assertEquals(mzx, calibrator.getInitialMzx(), 0.0);
28533         assertEquals(mzy, calibrator.getInitialMzy(), 0.0);
28534     }
28535 
28536     @Test
28537     void testGetSetBias() throws LockedException {
28538         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28539 
28540         // check default value
28541         final var bias1 = calibrator.getBias();
28542         assertArrayEquals(new double[3], bias1, 0.0);
28543 
28544         // set new values
28545         final var ba = generateBa();
28546         final var bias2 = ba.getBuffer();
28547         calibrator.setBias(bias2);
28548 
28549         // check
28550         final var bias3 = calibrator.getBias();
28551         final var bias4 = new double[3];
28552         calibrator.getBias(bias4);
28553 
28554         assertArrayEquals(bias2, bias3, 0.0);
28555         assertArrayEquals(bias2, bias4, 0.0);
28556 
28557         // Force IllegalArgumentException
28558         assertThrows(IllegalArgumentException.class, () -> calibrator.getBias(new double[1]));
28559         assertThrows(IllegalArgumentException.class, () -> calibrator.setBias(new double[1]));
28560     }
28561 
28562     @Test
28563     void testGetSetBiasAsMatrix() throws WrongSizeException, LockedException {
28564 
28565         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28566 
28567         // check default value
28568         final var bias1 = calibrator.getBiasAsMatrix();
28569         assertEquals(new Matrix(3, 1), bias1);
28570 
28571         // set new value
28572         final var bias2 = generateBa();
28573         calibrator.setBias(bias2);
28574 
28575         // check
28576         final var bias3 = calibrator.getBiasAsMatrix();
28577         final var bias4 = new Matrix(3, 1);
28578         calibrator.getBiasAsMatrix(bias4);
28579 
28580         assertEquals(bias2, bias3);
28581         assertEquals(bias2, bias4);
28582 
28583         // Force IllegalArgumentException
28584         final var m1 = new Matrix(1, 1);
28585         assertThrows(IllegalArgumentException.class, () -> calibrator.getBiasAsMatrix(m1));
28586         final var m2 = new Matrix(3, 3);
28587         assertThrows(IllegalArgumentException.class, () -> calibrator.getBiasAsMatrix(m2));
28588         final var m3 = new Matrix(1, 1);
28589         assertThrows(IllegalArgumentException.class, () -> calibrator.setBias(m3));
28590         final var m4 = new Matrix(3, 3);
28591         assertThrows(IllegalArgumentException.class, () -> calibrator.setBias(m4));
28592     }
28593 
28594     @Test
28595     void testGetSetInitialMa() throws WrongSizeException, LockedException {
28596         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28597 
28598         // test default value
28599         final var ma1 = calibrator.getInitialMa();
28600         assertEquals(new Matrix(3, 3), ma1);
28601 
28602         // set new value
28603         final var ma2 = generateMaGeneral();
28604         calibrator.setInitialMa(ma2);
28605 
28606         // check
28607         final var ma3 = calibrator.getInitialMa();
28608         final var ma4 = new Matrix(3, 3);
28609         calibrator.getInitialMa(ma4);
28610 
28611         assertEquals(ma2, ma3);
28612         assertEquals(ma2, ma4);
28613 
28614         // Force IllegalArgumentException
28615         final var m1 = new Matrix(1, 3);
28616         assertThrows(IllegalArgumentException.class, () -> calibrator.getInitialMa(m1));
28617         final var m2 = new Matrix(3, 1);
28618         assertThrows(IllegalArgumentException.class, () -> calibrator.getInitialMa(m2));
28619         final var m3 = new Matrix(1, 3);
28620         assertThrows(IllegalArgumentException.class, () -> calibrator.setInitialMa(m3));
28621         final var m4 = new Matrix(3, 1);
28622         assertThrows(IllegalArgumentException.class, () -> calibrator.setInitialMa(m4));
28623     }
28624 
28625     @Test
28626     void testGetSetMeasurements() throws LockedException {
28627         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28628 
28629         // check default value
28630         assertNull(calibrator.getMeasurements());
28631 
28632         // set new value
28633         final var measurements = Collections.<StandardDeviationBodyKinematics>emptyList();
28634         calibrator.setMeasurements(measurements);
28635 
28636         // check
28637         assertSame(measurements, calibrator.getMeasurements());
28638     }
28639 
28640     @Test
28641     void testIsSetCommonAxisUsed() throws LockedException {
28642         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28643 
28644         // check default value
28645         assertFalse(calibrator.isCommonAxisUsed());
28646 
28647         // set new value
28648         calibrator.setCommonAxisUsed(true);
28649 
28650         // check
28651         assertTrue(calibrator.isCommonAxisUsed());
28652     }
28653 
28654     @Test
28655     void testGetSetListener() throws LockedException {
28656         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28657 
28658         // check default value
28659         assertNull(calibrator.getListener());
28660 
28661         // set new value
28662         calibrator.setListener(this);
28663 
28664         // check
28665         assertSame(this, calibrator.getListener());
28666     }
28667 
28668     @Test
28669     void testGetSetGroundTruthGravityNorm1() throws LockedException {
28670         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28671 
28672         // check default value
28673         assertNull(calibrator.getGroundTruthGravityNorm());
28674 
28675         // set new value
28676         final var randomizer = new UniformRandomizer();
28677         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28678         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28679         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28680         final var nedPosition = new NEDPosition(latitude, longitude, height);
28681         final var nedVelocity = new NEDVelocity();
28682         final var ecefPosition = new ECEFPosition();
28683         final var ecefVelocity = new ECEFVelocity();
28684         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28685         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28686                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28687         final var gravityNorm = gravity.getNorm();
28688 
28689         calibrator.setGroundTruthGravityNorm(gravityNorm);
28690 
28691         // check
28692         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNorm(), 0.0);
28693         assertEquals(gravity.getNormAsAcceleration(), calibrator.getGroundTruthGravityNormAsAcceleration());
28694         final var g = new Acceleration(0.0, AccelerationUnit.G);
28695         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28696         assertEquals(g, gravity.getNormAsAcceleration());
28697     }
28698 
28699     @Test
28700     void testGetSetGroundTruthGravityNorm2() throws LockedException {
28701         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28702 
28703         // check default value
28704         assertNull(calibrator.getGroundTruthGravityNorm());
28705 
28706         // set new value
28707         final var randomizer = new UniformRandomizer();
28708         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28709         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28710         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28711         final var nedPosition = new NEDPosition(latitude, longitude, height);
28712         final var nedVelocity = new NEDVelocity();
28713         final var ecefPosition = new ECEFPosition();
28714         final var ecefVelocity = new ECEFVelocity();
28715         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28716         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28717                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28718         final var gravityNorm = gravity.getNormAsAcceleration();
28719 
28720         calibrator.setGroundTruthGravityNorm(gravityNorm);
28721 
28722         // check
28723         assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), 0.0);
28724         assertEquals(gravityNorm, calibrator.getGroundTruthGravityNormAsAcceleration());
28725         final var g = new Acceleration(0.0, AccelerationUnit.G);
28726         assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28727         assertEquals(gravityNorm, g);
28728     }
28729 
28730     @Test
28731     void testIsReady() throws LockedException {
28732         final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator();
28733 
28734         // check
28735         assertFalse(calibrator.isReady());
28736 
28737         // set empty measurements
28738         final var measurements1 = Collections.<StandardDeviationBodyKinematics>emptyList();
28739         calibrator.setMeasurements(measurements1);
28740 
28741         // check
28742         assertFalse(calibrator.isReady());
28743 
28744         // set enough measurements for general case
28745         calibrator.setCommonAxisUsed(false);
28746 
28747         final var measurements2 = new ArrayList<StandardDeviationBodyKinematics>();
28748         for (int i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
28749             measurements2.add(new StandardDeviationBodyKinematics());
28750         }
28751         calibrator.setMeasurements(measurements2);
28752 
28753         // check
28754         assertFalse(calibrator.isReady());
28755 
28756         // set gravity norm
28757         final var randomizer = new UniformRandomizer();
28758         final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28759         final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28760         final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28761         final var nedPosition = new NEDPosition(latitude, longitude, height);
28762         final var nedVelocity = new NEDVelocity();
28763         final var ecefPosition = new ECEFPosition();
28764         final var ecefVelocity = new ECEFVelocity();
28765         NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28766         final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28767                 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28768 
28769         calibrator.setGroundTruthGravityNorm(gravity.getNorm());
28770 
28771         assertTrue(calibrator.isReady());
28772 
28773         // set enough measurements for common axis case
28774         measurements2.clear();
28775         for (var i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS; i++) {
28776             measurements2.add(new StandardDeviationBodyKinematics());
28777         }
28778         calibrator.setMeasurements(measurements2);
28779 
28780         // check
28781         assertFalse(calibrator.isReady());
28782 
28783         calibrator.setCommonAxisUsed(true);
28784 
28785         assertTrue(calibrator.isReady());
28786     }
28787 
28788     @Test
28789     void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise() throws WrongSizeException,
28790             InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
28791 
28792         var numValid = 0;
28793         for (var t = 0; t < TIMES; t++) {
28794             final var ba = generateBa();
28795             final var bg = generateBg();
28796             final var ma = generateMaGeneral();
28797             final var mg = generateMg();
28798             final var gg = generateGg();
28799             // when using minimum number of measurements we must not add any noise so that
28800             // a solution is found, when adding more measurements, certain noise can be added
28801             final var accelNoiseRootPSD = 0.0;
28802             final var gyroNoiseRootPSD = 0.0;
28803             final var accelQuantLevel = 0.0;
28804             final var gyroQuantLevel = 0.0;
28805 
28806             final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
28807                     gyroQuantLevel);
28808 
28809             final var randomizer = new UniformRandomizer();
28810             final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28811             final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28812             final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28813             final var nedPosition = new NEDPosition(latitude, longitude, height);
28814             final var nedVelocity = new NEDVelocity();
28815             final var ecefPosition = new ECEFPosition();
28816             final var ecefVelocity = new ECEFVelocity();
28817             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28818             final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28819                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28820 
28821             final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
28822             final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
28823             final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
28824 
28825             final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
28826             for (var i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
28827                 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28828                 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28829                 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28830                 final var nedC = new CoordinateTransformation(roll, pitch, yaw,
28831                         FrameType.BODY_FRAME, FrameType.LOCAL_NAVIGATION_FRAME);
28832 
28833                 final var nedFrame = new NEDFrame(nedPosition, nedC);
28834                 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
28835 
28836                 // compute ground-truth kinematics that should be generated at provided
28837                 // position, velocity and orientation
28838                 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(
28839                         TIME_INTERVAL_SECONDS, ecefFrame, ecefFrame);
28840 
28841                 // apply known calibration parameters to distort ground-truth and generate a
28842                 // measured kinematics sample
28843                 final var random = new Random();
28844                 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
28845                         errors, random);
28846 
28847                 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
28848                         specificForceStandardDeviation, angularRateStandardDeviation);
28849                 measurements.add(measurement);
28850             }
28851 
28852             // When we have the minimum number of measurements, we need to provide
28853             // an initial solution close to the true solution
28854             final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
28855                     false, ba, ma, this);
28856 
28857             // estimate
28858             reset();
28859             assertTrue(calibrator.isReady());
28860             assertFalse(calibrator.isRunning());
28861             assertEquals(0, calibrateStart);
28862             assertEquals(0, calibrateEnd);
28863 
28864             try {
28865                 calibrator.calibrate();
28866             } catch (final CalibrationException e) {
28867                 continue;
28868             }
28869 
28870             // check
28871             assertTrue(calibrator.isReady());
28872             assertFalse(calibrator.isRunning());
28873             assertEquals(1, calibrateStart);
28874             assertEquals(1, calibrateEnd);
28875 
28876             final var estimatedMa = calibrator.getEstimatedMa();
28877 
28878             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
28879                 continue;
28880             }
28881 
28882             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
28883 
28884             assertEstimatedResult(estimatedMa, calibrator);
28885 
28886             assertNotNull(calibrator.getEstimatedCovariance());
28887             checkGeneralCovariance(calibrator.getEstimatedCovariance());
28888             assertTrue(calibrator.getEstimatedMse() >= 0.0);
28889 
28890             numValid++;
28891             break;
28892         }
28893 
28894         assertTrue(numValid > 0);
28895     }
28896 
28897     @Test
28898     void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise() throws WrongSizeException,
28899             InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
28900 
28901         var numValid = 0;
28902         for (var t = 0; t < TIMES; t++) {
28903             final var ba = generateBa();
28904             final var bg = generateBg();
28905             final var ma = generateMaGeneral();
28906             final var mg = generateMg();
28907             final var gg = generateGg();
28908             final var accelNoiseRootPSD = getAccelNoiseRootPSD();
28909             final var gyroNoiseRootPSD = getGyroNoiseRootPSD();
28910             final var accelQuantLevel = 0.0;
28911             final var gyroQuantLevel = 0.0;
28912 
28913             final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
28914                     gyroQuantLevel);
28915 
28916             final var randomizer = new UniformRandomizer();
28917             final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
28918             final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28919             final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28920             final var nedPosition = new NEDPosition(latitude, longitude, height);
28921             final var nedVelocity = new NEDVelocity();
28922             final var ecefPosition = new ECEFPosition();
28923             final var ecefVelocity = new ECEFVelocity();
28924             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
28925             final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28926                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28927 
28928             final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
28929             final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
28930             final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
28931 
28932             final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
28933             for (var i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
28934                 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28935                 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28936                 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
28937                 final var nedC = new CoordinateTransformation(roll, pitch, yaw, FrameType.BODY_FRAME,
28938                         FrameType.LOCAL_NAVIGATION_FRAME);
28939 
28940                 final var nedFrame = new NEDFrame(nedPosition, nedC);
28941                 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
28942 
28943                 // compute ground-truth kinematics that should be generated at provided
28944                 // position, velocity and orientation
28945                 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS,
28946                         ecefFrame, ecefFrame);
28947 
28948                 // apply known calibration parameters to distort ground-truth and generate a
28949                 // measured kinematics sample
28950                 final var random = new Random();
28951                 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
28952                         errors, random);
28953 
28954                 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
28955                         specificForceStandardDeviation, angularRateStandardDeviation);
28956                 measurements.add(measurement);
28957             }
28958 
28959             // When we have a large number of measurements, we do not need to provide
28960             // an initial solution as it will probably converge to true solution
28961             final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
28962                     false, this);
28963 
28964             // estimate
28965             reset();
28966             assertTrue(calibrator.isReady());
28967             assertFalse(calibrator.isRunning());
28968             assertEquals(0, calibrateStart);
28969             assertEquals(0, calibrateEnd);
28970 
28971             try {
28972                 calibrator.calibrate();
28973             } catch (final CalibrationException e) {
28974                 continue;
28975             }
28976 
28977             // check
28978             assertTrue(calibrator.isReady());
28979             assertFalse(calibrator.isRunning());
28980             assertEquals(1, calibrateStart);
28981             assertEquals(1, calibrateEnd);
28982 
28983             final var estimatedMa = calibrator.getEstimatedMa();
28984 
28985             if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
28986                 continue;
28987             }
28988 
28989             assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
28990 
28991             assertEstimatedResult(estimatedMa, calibrator);
28992 
28993             assertNotNull(calibrator.getEstimatedCovariance());
28994             checkGeneralCovariance(calibrator.getEstimatedCovariance());
28995             assertTrue(calibrator.getEstimatedMse() > 0.0);
28996 
28997             numValid++;
28998             break;
28999         }
29000 
29001         assertTrue(numValid > 0);
29002     }
29003 
29004     @Test
29005     void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise() throws WrongSizeException,
29006             InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
29007 
29008         var numValid = 0;
29009         for (var t = 0; t < TIMES; t++) {
29010             final var ba = generateBa();
29011             final var bg = generateBg();
29012             final var ma = generateMaCommonAxis();
29013             final var mg = generateMg();
29014             final var gg = generateGg();
29015             // when using minimum number of measurements we must not add any noise so that
29016             // a solution is found, when adding more measurements, certain noise can be added
29017             final var accelNoiseRootPSD = 0.0;
29018             final var gyroNoiseRootPSD = 0.0;
29019             final var accelQuantLevel = 0.0;
29020             final var gyroQuantLevel = 0.0;
29021 
29022             final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
29023                     gyroQuantLevel);
29024 
29025             final var randomizer = new UniformRandomizer();
29026             final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29027             final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29028             final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29029             final var nedPosition = new NEDPosition(latitude, longitude, height);
29030             final var nedVelocity = new NEDVelocity();
29031             final var ecefPosition = new ECEFPosition();
29032             final var ecefVelocity = new ECEFVelocity();
29033             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
29034             final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29035                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29036 
29037             final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29038             final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29039             final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29040 
29041             final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
29042             for (var i = 0; i < KnownBiasAndGravityNormAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMMON_Z_AXIS; i++) {
29043                 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29044                 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29045                 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29046                 final var nedC = new CoordinateTransformation(roll, pitch, yaw, FrameType.BODY_FRAME,
29047                         FrameType.LOCAL_NAVIGATION_FRAME);
29048 
29049                 final var nedFrame = new NEDFrame(nedPosition, nedC);
29050                 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
29051 
29052                 // compute ground-truth kinematics that should be generated at provided
29053                 // position, velocity and orientation
29054                 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS,
29055                         ecefFrame, ecefFrame);
29056 
29057                 // apply known calibration parameters to distort ground-truth and generate a
29058                 // measured kinematics sample
29059                 final var random = new Random();
29060                 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
29061                         errors, random);
29062 
29063                 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
29064                         specificForceStandardDeviation, angularRateStandardDeviation);
29065                 measurements.add(measurement);
29066             }
29067 
29068             // When we have the minimum number of measurements, we need to provide
29069             // an initial solution close to the true solution
29070             final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
29071                     true, ba, ma, this);
29072 
29073             // estimate
29074             reset();
29075             assertTrue(calibrator.isReady());
29076             assertFalse(calibrator.isRunning());
29077             assertEquals(0, calibrateStart);
29078             assertEquals(0, calibrateEnd);
29079 
29080             try {
29081                 calibrator.calibrate();
29082             } catch (final CalibrationException e) {
29083                 continue;
29084             }
29085 
29086             // check
29087             assertTrue(calibrator.isReady());
29088             assertFalse(calibrator.isRunning());
29089             assertEquals(1, calibrateStart);
29090             assertEquals(1, calibrateEnd);
29091 
29092             final var estimatedMa = calibrator.getEstimatedMa();
29093 
29094             if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
29095                 continue;
29096             }
29097 
29098             assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
29099 
29100             assertEstimatedResult(estimatedMa, calibrator);
29101 
29102             assertNotNull(calibrator.getEstimatedCovariance());
29103             checkCommonAxisCovariance(calibrator.getEstimatedCovariance());
29104             assertTrue(calibrator.getEstimatedMse() >= 0.0);
29105 
29106             numValid++;
29107 
29108             break;
29109         }
29110 
29111         assertTrue(numValid > 0);
29112     }
29113 
29114     @Test
29115     void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise() throws WrongSizeException,
29116             InvalidSourceAndDestinationFrameTypeException, LockedException, NotReadyException {
29117 
29118         var numValid = 0;
29119         for (var t = 0; t < TIMES; t++) {
29120             final var ba = generateBa();
29121             final var bg = generateBg();
29122             final var ma = generateMaCommonAxis();
29123             final var mg = generateMg();
29124             final var gg = generateGg();
29125             final var accelNoiseRootPSD = getAccelNoiseRootPSD();
29126             final var gyroNoiseRootPSD = getGyroNoiseRootPSD();
29127             final var accelQuantLevel = 0.0;
29128             final var gyroQuantLevel = 0.0;
29129 
29130             final var errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD, gyroNoiseRootPSD, accelQuantLevel,
29131                     gyroQuantLevel);
29132 
29133             final var randomizer = new UniformRandomizer();
29134             final var latitude = Math.toRadians(randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREES));
29135             final var longitude = Math.toRadians(randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29136             final var height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29137             final var nedPosition = new NEDPosition(latitude, longitude, height);
29138             final var nedVelocity = new NEDVelocity();
29139             final var ecefPosition = new ECEFPosition();
29140             final var ecefVelocity = new ECEFVelocity();
29141             NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity, ecefPosition, ecefVelocity);
29142             final var gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29143                     ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29144 
29145             final var sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
29146             final var specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
29147             final var angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
29148 
29149             final var measurements = new ArrayList<StandardDeviationBodyKinematics>();
29150             for (var i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
29151                 final var roll = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29152                 final var pitch = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29153                 final var yaw = Math.toRadians(randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
29154                 final var nedC = new CoordinateTransformation(roll, pitch, yaw, FrameType.BODY_FRAME,
29155                         FrameType.LOCAL_NAVIGATION_FRAME);
29156 
29157                 final var nedFrame = new NEDFrame(nedPosition, nedC);
29158                 final var ecefFrame = NEDtoECEFFrameConverter.convertNEDtoECEFAndReturnNew(nedFrame);
29159 
29160                 // compute ground-truth kinematics that should be generated at provided
29161                 // position, velocity and orientation
29162                 final var trueKinematics = ECEFKinematicsEstimator.estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS,
29163                         ecefFrame, ecefFrame);
29164 
29165                 // apply known calibration parameters to distort ground-truth and generate a
29166                 // measured kinematics sample
29167                 final var random = new Random();
29168                 final var measuredKinematics = BodyKinematicsGenerator.generate(TIME_INTERVAL_SECONDS, trueKinematics,
29169                         errors, random);
29170 
29171                 final var measurement = new StandardDeviationBodyKinematics(measuredKinematics,
29172                         specificForceStandardDeviation, angularRateStandardDeviation);
29173                 measurements.add(measurement);
29174             }
29175 
29176             // When we have a large number of measurements, we do not need to provide
29177             // an initial solution as it will probably converge to true solution
29178             final var calibrator = new KnownBiasAndGravityNormAccelerometerCalibrator(gravity.getNorm(), measurements,
29179                     true, this);
29180 
29181             // estimate
29182             reset();
29183             assertTrue(calibrator.isReady());
29184             assertFalse(calibrator.isRunning());
29185             assertEquals(0, calibrateStart);
29186             assertEquals(0, calibrateEnd);
29187 
29188             try {
29189                 calibrator.calibrate();
29190             } catch (final CalibrationException e) {
29191                 continue;
29192             }
29193 
29194             // check
29195             assertTrue(calibrator.isReady());
29196             assertFalse(calibrator.isRunning());
29197             assertEquals(1, calibrateStart);
29198             assertEquals(1, calibrateEnd);
29199 
29200             final var estimatedMa = calibrator.getEstimatedMa();
29201 
29202             if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
29203                 continue;
29204             }
29205 
29206             assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
29207 
29208             assertEstimatedResult(estimatedMa, calibrator);
29209 
29210             assertNotNull(calibrator.getEstimatedCovariance());
29211             checkCommonAxisCovariance(calibrator.getEstimatedCovariance());
29212             assertTrue(calibrator.getEstimatedMse() > 0.0);
29213 
29214             numValid++;
29215             break;
29216         }
29217 
29218         assertTrue(numValid > 0);
29219     }
29220 
29221     @Override
29222     public void onCalibrateStart(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29223         checkLocked(calibrator);
29224         calibrateStart++;
29225     }
29226 
29227     @Override
29228     public void onCalibrateEnd(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29229         checkLocked(calibrator);
29230         calibrateEnd++;
29231     }
29232 
29233     private void reset() {
29234         calibrateStart = 0;
29235         calibrateEnd = 0;
29236     }
29237 
29238     private void checkLocked(final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29239         assertTrue(calibrator.isRunning());
29240         assertThrows(LockedException.class, () -> calibrator.setBiasX(0.0));
29241         assertThrows(LockedException.class, () -> calibrator.setBiasY(0.0));
29242         assertThrows(LockedException.class, () -> calibrator.setBiasZ(0.0));
29243         assertThrows(LockedException.class, () -> calibrator.setBiasX(null));
29244         assertThrows(LockedException.class, () -> calibrator.setBiasY(null));
29245         assertThrows(LockedException.class, () -> calibrator.setBiasZ(null));
29246         assertThrows(LockedException.class, () -> calibrator.setBiasCoordinates(0.0, 0.0, 0.0));
29247         assertThrows(LockedException.class, () -> calibrator.setBiasCoordinates(null, null, null));
29248         assertThrows(LockedException.class, () -> calibrator.setBias((AccelerationTriad) null));
29249         assertThrows(LockedException.class, () -> calibrator.setInitialSx(0.0));
29250         assertThrows(LockedException.class, () -> calibrator.setInitialSy(0.0));
29251         assertThrows(LockedException.class, () -> calibrator.setInitialSz(0.0));
29252         assertThrows(LockedException.class, () -> calibrator.setInitialMxy(0.0));
29253         assertThrows(LockedException.class, () -> calibrator.setInitialMxz(0.0));
29254         assertThrows(LockedException.class, () -> calibrator.setInitialMyx(0.0));
29255         assertThrows(LockedException.class, () -> calibrator.setInitialMyz(0.0));
29256         assertThrows(LockedException.class, () -> calibrator.setInitialMzx(0.0));
29257         assertThrows(LockedException.class, () -> calibrator.setInitialMzy(0.0));
29258         assertThrows(LockedException.class, () -> calibrator.setInitialScalingFactors(
29259                 0.0, 0.0, 0.0));
29260         assertThrows(LockedException.class, () -> calibrator.setInitialCrossCouplingErrors(
29261                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0));
29262         assertThrows(LockedException.class, () -> calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
29263                 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
29264                 0.0, 0.0, 0.0));
29265         assertThrows(LockedException.class, () -> calibrator.setBias((double[]) null));
29266         assertThrows(LockedException.class, () -> calibrator.setBias((Matrix) null));
29267         assertThrows(LockedException.class, () -> calibrator.setInitialMa(null));
29268         assertThrows(LockedException.class, () -> calibrator.setGroundTruthGravityNorm(0.0));
29269         assertThrows(LockedException.class, () -> calibrator.setGroundTruthGravityNorm((Acceleration) null));
29270         assertThrows(LockedException.class, () -> calibrator.setMeasurements(null));
29271         assertThrows(LockedException.class, () -> calibrator.setCommonAxisUsed(true));
29272         assertThrows(LockedException.class, () -> calibrator.setListener(this));
29273         assertThrows(LockedException.class, calibrator::calibrate);
29274     }
29275 
29276     private static void assertEstimatedResult(
29277             final Matrix ma, final KnownBiasAndGravityNormAccelerometerCalibrator calibrator) {
29278 
29279         assertEquals(ma.getElementAt(0, 0), calibrator.getEstimatedSx(), 0.0);
29280         assertEquals(ma.getElementAt(1, 1), calibrator.getEstimatedSy(), 0.0);
29281         assertEquals(ma.getElementAt(2, 2), calibrator.getEstimatedSz(), 0.0);
29282         assertEquals(ma.getElementAt(0, 1), calibrator.getEstimatedMxy(), 0.0);
29283         assertEquals(ma.getElementAt(0, 2), calibrator.getEstimatedMxz(), 0.0);
29284         assertEquals(ma.getElementAt(1, 0), calibrator.getEstimatedMyx(), 0.0);
29285         assertEquals(ma.getElementAt(1, 2), calibrator.getEstimatedMyz(), 0.0);
29286         assertEquals(ma.getElementAt(2, 0), calibrator.getEstimatedMzx(), 0.0);
29287         assertEquals(ma.getElementAt(2, 1), calibrator.getEstimatedMzy(), 0.0);
29288     }
29289 
29290     private static void checkCommonAxisCovariance(final Matrix covariance) {
29291         assertEquals(9, covariance.getRows());
29292         assertEquals(9, covariance.getColumns());
29293 
29294         for (var j = 0; j < 9; j++) {
29295             final var colIsZero = j == 5 || j == 7 || j == 8;
29296             for (var i = 0; i < 9; i++) {
29297                 final var rowIsZero = i == 5 || i == 7 || i == 8;
29298                 if (colIsZero || rowIsZero) {
29299                     assertEquals(0.0, covariance.getElementAt(i, j), 0.0);
29300                 }
29301             }
29302         }
29303     }
29304 
29305     private static void checkGeneralCovariance(final Matrix covariance) {
29306         assertEquals(9, covariance.getRows());
29307         assertEquals(9, covariance.getColumns());
29308 
29309         for (var i = 0; i < 9; i++) {
29310             assertNotEquals(0.0, covariance.getElementAt(i, i));
29311         }
29312     }
29313 
29314     private static Matrix generateBa() {
29315         return Matrix.newFromArray(new double[]{
29316                 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
29317                 -1000 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
29318                 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
29319     }
29320 
29321     private static Matrix generateBg() {
29322         return Matrix.newFromArray(new double[]{
29323                 -9 * DEG_TO_RAD / 3600.0,
29324                 10 * DEG_TO_RAD / 3600.0,
29325                 -8 * DEG_TO_RAD / 3600.0});
29326     }
29327 
29328     private static Matrix generateMaGeneral() throws WrongSizeException {
29329         final var result = new Matrix(3, 3);
29330         result.fromArray(new double[]{
29331                 500e-6, -300e-6, 200e-6,
29332                 -150e-6, -600e-6, 250e-6,
29333                 -250e-6, 70e-6, 450e-6
29334         }, false);
29335 
29336         return result;
29337     }
29338 
29339     private static Matrix generateMaCommonAxis() throws WrongSizeException {
29340         final var result = new Matrix(3, 3);
29341         result.fromArray(new double[]{
29342                 500e-6, -300e-6, 200e-6,
29343                 0.0, -600e-6, 250e-6,
29344                 0.0, 0.0, 450e-6
29345         }, false);
29346 
29347         return result;
29348     }
29349 
29350     private static Matrix generateMg() throws WrongSizeException {
29351         final var result = new Matrix(3, 3);
29352         result.fromArray(new double[]{
29353                 400e-6, -300e-6, 250e-6,
29354                 0.0, -300e-6, -150e-6,
29355                 0.0, 0.0, -350e-6
29356         }, false);
29357 
29358         return result;
29359     }
29360 
29361     private static Matrix generateGg() throws WrongSizeException {
29362         final var result = new Matrix(3, 3);
29363         final var tmp = DEG_TO_RAD / (3600 * 9.80665);
29364         result.fromArray(new double[]{
29365                 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
29366                 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
29367                 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
29368         }, false);
29369 
29370         return result;
29371     }
29372 
29373     private static double getAccelNoiseRootPSD() {
29374         return 70.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
29375     }
29376 
29377     private static double getGyroNoiseRootPSD() {
29378         return 0.01 * DEG_TO_RAD / 60.0;
29379     }
29380 }